/*
 * VesselGraph.cpp
 *
 *  Created on: 13.04.2010
 *      Author: jagiella
 */

//#include <cstdio>
#include <fstream>
#include <stdlib.h>
#include <math.h>
#include <omp.h>

#include "VesselGraph.hpp"
#include "Mathematix.h"
#include <PovrayIO.h>
#include <EpsIO.h>
#include "SparseMatrix.h"
#include "Solver.h"
#include "octree.h"



VesselGraph::VesselGraph()
{
	this->countVesselNodes = 0;
	this->countVesselSegments = 0;
	this->maxVesselNodes = 0;
	this->maxVesselSegments = 0;
	this->vesselNodes = 0;
	this->vesselSegments = 0;
	
	fprintf( stderr, "Octree Size: %ix%ix%i\n",
			(int)pow( 2, (int)ceil( log(DOMAIN_SIZE_X)/log(2))),
			(int)pow( 2, (int)ceil( log(DOMAIN_SIZE_Y)/log(2))),
			(int)pow( 2, (int)ceil( log(DOMAIN_SIZE_Z)/log(2))));
	octree = new Octree<VesselNode*>( 
			MAX( pow(2,(int)ceil( log(DOMAIN_SIZE_X)/log(2))), MAX(pow(2,(int)ceil( log(DOMAIN_SIZE_Y)/log(2))), pow(2,(int)ceil( log(DOMAIN_SIZE_Z)/log(2)))))
					);
}

VesselGraph::~VesselGraph()
{
	delete octree;
	for( int i=0; i<this->countVesselSegments; i++)
		delete this->vesselSegments[i];
	free(this->vesselSegments);
	for( int i=0; i<this->countVesselNodes; i++)
		delete this->vesselNodes[i];
	free(this->vesselNodes);
}

void VesselGraph::addVesselNode( VesselNode *vn)
{
	if(countVesselNodes == maxVesselNodes){
		vesselNodes = (VesselNode**) realloc( vesselNodes, sizeof(VesselNode*) * (maxVesselNodes+10));
		maxVesselNodes+=10;
	}

	vesselNodes[countVesselNodes] = vn;
	vesselNodes[countVesselNodes]->index = countVesselNodes;
	countVesselNodes++;
	
	octree->set(vn->position[0], vn->position[1], vn->position[2], vn);
}

void VesselGraph::removeVesselNode( VesselNode *vn)
{
	countVesselNodes--;
	//fprintf( stderr, "remove Vessel Node %i\n", vn->index);
	vesselNodes[vn->index] = vesselNodes[countVesselNodes];
	vesselNodes[vn->index]->index = vn->index;

	octree->erase(vn->position[0], vn->position[1], vn->position[2]);
}

void VesselGraph::addVesselSegment( VesselSegment *vs)
{
	if(countVesselSegments == maxVesselSegments){
		vesselSegments = (VesselSegment**) realloc( vesselSegments, sizeof(VesselSegment*) * (maxVesselSegments+10));
		maxVesselSegments+=10;
	}

	vesselSegments[countVesselSegments] = vs;
	vs->index = countVesselSegments;
	countVesselSegments++;
}

void VesselGraph::addVesselSegmentAndNeighbors( VesselSegment *vs)
{
	if(countVesselSegments == maxVesselSegments){
		vesselSegments = (VesselSegment**) realloc( vesselSegments, sizeof(VesselSegment*) * (maxVesselSegments+10));
		maxVesselSegments+=10;
	}

	vs->vesselNodes[0]->addNeighbor( vs->vesselNodes[1], vs);
	vs->vesselNodes[1]->addNeighbor( vs->vesselNodes[0], vs);

	vesselSegments[countVesselSegments] = vs;
	vs->index = countVesselSegments;
	countVesselSegments++;
}

void VesselGraph::addVesselSegmentAndNeighbors( VesselSegment *vs, float radius)
{
	if(countVesselSegments == maxVesselSegments){
		vesselSegments = (VesselSegment**) realloc( vesselSegments, sizeof(VesselSegment*) * (maxVesselSegments+10));
		maxVesselSegments+=10;
	}

	vs->vesselNodes[0]->addNeighbor( vs->vesselNodes[1], vs);
	vs->vesselNodes[1]->addNeighbor( vs->vesselNodes[0], vs);

	vesselSegments[countVesselSegments] = vs;
	vs->index = countVesselSegments;
	countVesselSegments++;
	
	vs->radius = radius;
}

void VesselGraph::removeVesselSegmentAndNeighbors( VesselSegment *vs)
{
	countVesselSegments--;

	vesselSegments[vs->index] = vesselSegments[countVesselSegments];
	vesselSegments[vs->index]->index = vs->index;

	vs->vesselNodes[0]->removeNeighbor( vs->vesselNodes[1]);
	vs->vesselNodes[1]->removeNeighbor( vs->vesselNodes[0]);
}

void VesselGraph::removeVesselSegment( VesselSegment *vs)
{
	countVesselSegments--;

	vesselSegments[vs->index] = vesselSegments[countVesselSegments];
	vesselSegments[vs->index]->index = vs->index;
	
	// NEW
/*	VesselSegment *removedVS = vs;
	VesselNode *removedVN0 = removedVS->vesselNodes[0];
	VesselNode *removedVN1 = removedVS->vesselNodes[1];
	
	this->removeVesselSegment( removedVS);
	delete removedVS;

	if( removedVN0->countNeighbors == 0 && removedVN0->getType()!=ROOT){
		VesselNode *removedVN = removedVN0;
		this->removeVesselNode( removedVN);
		delete removedVN;
	}
	if( removedVN1->countNeighbors == 0 && removedVN1->getType()!=ROOT){
		VesselNode *removedVN = removedVN1;
		this->removeVesselNode( removedVN);
		delete removedVN;
	}*/
}

void VesselGraph::removeVesselSegmentAndNodes( VesselSegment *vs)
{
	countVesselSegments--;

	vesselSegments[vs->index] = vesselSegments[countVesselSegments];
	vesselSegments[vs->index]->index = vs->index;

	// NEW
	VesselSegment *removedVS = vs;
	VesselNode *removedVN0 = removedVS->vesselNodes[0];
	VesselNode *removedVN1 = removedVS->vesselNodes[1];

	removedVN0->removeNeighbor( removedVN1);
	removedVN1->removeNeighbor( removedVN0);

	//this->removeVesselSegment( removedVS);
	delete removedVS;

	if( removedVN0->countNeighbors == 0 && removedVN0->getType()!=ROOT){
		VesselNode *removedVN = removedVN0;
		this->removeVesselNode( removedVN);
		delete removedVN;
	}
	if( removedVN1->countNeighbors == 0 && removedVN1->getType()!=ROOT){
		VesselNode *removedVN = removedVN1;
		this->removeVesselNode( removedVN);
		delete removedVN;
	}
}

void VesselGraph::writeStatisticsToFile(const char *filename, char mode)
{
	int N=50;
	//bool ONLY_FUNCTIONAL_VESSELS = true;

	// Statistics
	double mean[2][N];
	double meanSquare[2][N];
	int count[2][N];

	for(int t=0;t<2;t++)
	for(int i=0; i<N; i++){
		mean[t][i]=0;
		meanSquare[t][i]=0;
		count[t][i]=0;
	}

	int arterie = 0;
	int vene = 1;
	int type;

	double dradius = 5.;
	int total = 0;
	for(int s=0; s<this->countVesselSegments; s++)
	//if(!ONLY_FUNCTIONAL_VESSELS || fabs(this->vesselSegments[s]->flow) > 1000.)
	{

		// interval
		int i = (int)(this->vesselSegments[s]->radius/dradius);

		// type
		if( this->vesselSegments[s]->type == ARTERIE)
			type = arterie;
		else
			type = vene;

		// parameter
		double parameter=0.;
		switch( mode){
		case 0:
			// pressure
			parameter = (this->vesselSegments[s]->vesselNodes[0]->pressure + this->vesselSegments[s]->vesselNodes[1]->pressure)*0.5;
			break;

		case 1:
			// flow
			parameter = fabs(this->vesselSegments[s]->flow);
			break;

		case 2:
			// velocity
			parameter = fabs(this->vesselSegments[s]->flow) / (PI * this->vesselSegments[s]->radius*this->vesselSegments[s]->radius);
			break;

		case 3:
			// shear stress
			parameter = this->vesselSegments[s]->shear;
			break;
		}

		mean      [type][i] += parameter;
		meanSquare[type][i] += parameter*parameter;
		count     [type][i]++;
		total++;
	}

	// Output
	FILE *fp = fopen( filename,"w+");
	fprintf(fp,
			"#min radius;"\
			"max radius;"\
			"type (-1=arterie, 1=vene);"\
			"parameter (type=%i);"\
			"std. deviation\n", (int)mode);

	for( int i=N-1;i>=0;i--)
		if(count[0][i]!=0){
			// Arteries
			fprintf(fp, "%lf %lf %e %e\n",
				-dradius*(i+0.5),
				-dradius*0.5,
				mean[0][i]/count[0][i],
				sqrt( meanSquare[0][i]/count[0][i] - mean[0][i]/count[0][i] * mean[0][i]/count[0][i]));
		}

	for( int i=0;i<N;i++)
		if(count[1][i]!=0){
			// Venes
			fprintf(fp, "%lf %lf %e %e\n",
				dradius*(i+0.5),
				dradius*0.5,
				mean[1][i]/count[1][i],
				sqrt( meanSquare[1][i]/count[1][i] - mean[1][i]/count[1][i] * mean[1][i]/count[1][i]));
		}

	fclose(fp);
}

void VesselGraph::printToPovray(const char *filename, char mode)
{
	std::fstream fs;
	fs.open( filename, std::fstream::out);

	PovrayIO::writePovrayHeader( &fs, 0, 0, DOMAIN_SIZE_X, DOMAIN_SIZE_Y);

	for( int s=0; s<this->countVesselSegments; s++){
		//float noise0[2] = {RAND01,RAND01};
		//float noise1[2] = {RAND01,RAND01};
		
		char color[512],color0[512],color1[512];
		/*sprintf( color0, "rgb<%lf ,0 ,%lf>",
				1.-(MAX_PRESSURE - this->vesselSegments[s]->vesselNodes[0]->pressure)/(MAX_PRESSURE - MIN_PRESSURE),
				(MAX_PRESSURE - this->vesselSegments[s]->vesselNodes[0]->pressure)/(MAX_PRESSURE - MIN_PRESSURE));
		sprintf( color1, "rgb<%lf ,0 ,%lf>", 
				1.-(MAX_PRESSURE - this->vesselSegments[s]->vesselNodes[1]->pressure)/(MAX_PRESSURE - MIN_PRESSURE),
				(MAX_PRESSURE - this->vesselSegments[s]->vesselNodes[1]->pressure)/(MAX_PRESSURE - MIN_PRESSURE));
		sprintf( color, "rgb<%lf ,0 ,%lf>",
				1.-(MAX_PRESSURE - 0.5*this->vesselSegments[s]->vesselNodes[0]->pressure - 0.5*this->vesselSegments[s]->vesselNodes[1]->pressure)/(MAX_PRESSURE - MIN_PRESSURE),
				(MAX_PRESSURE - 0.5*this->vesselSegments[s]->vesselNodes[0]->pressure - 0.5*this->vesselSegments[s]->vesselNodes[1]->pressure)/(MAX_PRESSURE - MIN_PRESSURE));
		 */
		switch( mode){
		case 1:
			// Marker Concentration
			sprintf( color0, "rgb<%lf ,%lf ,0>", this->vesselSegments[s]->vesselNodes[0]->marker, 1-this->vesselSegments[s]->vesselNodes[0]->marker);
			sprintf( color1, "rgb<%lf ,%lf ,0>", this->vesselSegments[s]->vesselNodes[1]->marker, 1-this->vesselSegments[s]->vesselNodes[1]->marker);
			sprintf( color, "rgb<%lf ,%lf ,0>", this->vesselSegments[s]->vesselNodes[0]->marker*0.5+this->vesselSegments[s]->vesselNodes[1]->marker*0.5, 1-this->vesselSegments[s]->vesselNodes[0]->marker*0.5-this->vesselSegments[s]->vesselNodes[1]->marker*0.5);
			break;
		case 0:
			// Pressure
			sprintf( color0, "rgb<%lf ,0 ,%lf>",
					1.-(MAX_PRESSURE - this->vesselSegments[s]->vesselNodes[0]->pressure)/(MAX_PRESSURE - MIN_PRESSURE),
					(MAX_PRESSURE - this->vesselSegments[s]->vesselNodes[0]->pressure)/(MAX_PRESSURE - MIN_PRESSURE));
			sprintf( color1, "rgb<%lf ,0 ,%lf>", 
					1.-(MAX_PRESSURE - this->vesselSegments[s]->vesselNodes[1]->pressure)/(MAX_PRESSURE - MIN_PRESSURE),
					(MAX_PRESSURE - this->vesselSegments[s]->vesselNodes[1]->pressure)/(MAX_PRESSURE - MIN_PRESSURE));
			sprintf( color, "rgb<%lf ,0 ,%lf>",
					1.-(MAX_PRESSURE - 0.5*this->vesselSegments[s]->vesselNodes[0]->pressure - 0.5*this->vesselSegments[s]->vesselNodes[1]->pressure)/(MAX_PRESSURE - MIN_PRESSURE),
					(MAX_PRESSURE - 0.5*this->vesselSegments[s]->vesselNodes[0]->pressure - 0.5*this->vesselSegments[s]->vesselNodes[1]->pressure)/(MAX_PRESSURE - MIN_PRESSURE));		
			break;
		}
		//sprintf( color, "rgb<1 ,0 ,0>");
		/*if( this->vesselSegments[s]->vesselNodes[0]->getType() == TIP) sprintf( color0, "rgb<0 ,0 ,1>");
		else if( this->vesselSegments[s]->vesselNodes[0]->getType() == ROOT) sprintf( color0, "rgb<0 ,1 ,0>");
		else sprintf( color0, "rgb<1 ,0 ,0>");
		if( this->vesselSegments[s]->vesselNodes[1]->getType() == TIP) sprintf( color1, "rgb<0 ,0 ,1>");
		else if( this->vesselSegments[s]->vesselNodes[1]->getType() == ROOT) sprintf( color0, "rgb<0 ,1 ,0>");
		else sprintf( color1, "rgb<1 ,0 ,0>");*/
		
		PovrayIO::writeCylinder( &fs,
				this->vesselSegments[s]->vesselNodes[0]->position[0],//+noise0[0],
				this->vesselSegments[s]->vesselNodes[0]->position[1],//+noise0[1],
				this->vesselSegments[s]->vesselNodes[0]->position[2],
				this->vesselSegments[s]->vesselNodes[1]->position[0],//+noise1[0],
				this->vesselSegments[s]->vesselNodes[1]->position[1],//+noise1[1],
				this->vesselSegments[s]->vesselNodes[1]->position[2],
				this->vesselSegments[s]->radius/LATTICE_CONSTANT, 
				color);
				//(this->vesselSegments[s]->vesselNodes[1]->pressure+this->vesselSegments[s]->vesselNodes[1]->pressure!=0? "Red":"Blue"));
		PovrayIO::writeSphere( &fs, 
				this->vesselSegments[s]->vesselNodes[0]->position[0],//+noise0[0], 
				this->vesselSegments[s]->vesselNodes[0]->position[1],//+noise0[0],
				this->vesselSegments[s]->vesselNodes[0]->position[2],//+noise0[1],
				this->vesselSegments[s]->radius/LATTICE_CONSTANT,
				//(this->vesselSegments[s]->vesselNodes[0]->getType() == TIP||this->vesselSegments[s]->vesselNodes[0]->getType() == ROOT?.3:this->vesselSegments[s]->radius/10.),
				color0);
				//(this->vesselSegments[s]->vesselNodes[0]->pressure!=0?"Red":"Blue"));
		PovrayIO::writeSphere( &fs, 
				this->vesselSegments[s]->vesselNodes[1]->position[0],//+noise1[0], 
				this->vesselSegments[s]->vesselNodes[1]->position[1],//+noise1[0],
				this->vesselSegments[s]->vesselNodes[1]->position[2],//+noise1[1],
				this->vesselSegments[s]->radius/LATTICE_CONSTANT,
				//(this->vesselSegments[s]->vesselNodes[1]->getType() == TIP||this->vesselSegments[s]->vesselNodes[1]->getType() == ROOT?.3:this->vesselSegments[s]->radius/10.),
				color1);
				//(this->vesselSegments[s]->vesselNodes[1]->pressure!=0?"Red":"Blue"));

	}

	//for( int s=0; s<this->countVesselNodes; s++)
	//	PovrayIO::writeSphere( &fs, this->vesselNodes[s]->position[0], this->vesselNodes[s]->position[1], 1, "Red");

	fs.close();
}


void VesselGraph::printToPovray(const char *filename, char mode, float ***marker)
{
	std::fstream fs;
	fs.open( filename, std::fstream::out);

	PovrayIO::writePovrayHeader( &fs, 0, 0, DOMAIN_SIZE_X, DOMAIN_SIZE_Y);

	for( int s=0; s<this->countVesselSegments; s++){
		//float noise0[2] = {RAND01,RAND01};
		//float noise1[2] = {RAND01,RAND01};

		char color[512],color0[512],color1[512];
		switch( mode){
		case 1:
			// Marker Concentration
			sprintf( color0, "rgb<1, %lf ,%lf>", 1-this->vesselSegments[s]->vesselNodes[0]->marker, 1-this->vesselSegments[s]->vesselNodes[0]->marker);
			sprintf( color1, "rgb<1, %lf ,%lf>", 1-this->vesselSegments[s]->vesselNodes[1]->marker, 1-this->vesselSegments[s]->vesselNodes[1]->marker);
			sprintf( color,  "rgb<1, %lf ,%lf>", 1-this->vesselSegments[s]->vesselNodes[0]->marker*0.5-this->vesselSegments[s]->vesselNodes[1]->marker*0.5, 1-this->vesselSegments[s]->vesselNodes[0]->marker*0.5-this->vesselSegments[s]->vesselNodes[1]->marker*0.5);
			break;
		case 0:
			// Pressure
			sprintf( color0, "rgb<%lf ,0 ,%lf>",
					1.-(MAX_PRESSURE - this->vesselSegments[s]->vesselNodes[0]->pressure)/(MAX_PRESSURE - MIN_PRESSURE),
					(MAX_PRESSURE - this->vesselSegments[s]->vesselNodes[0]->pressure)/(MAX_PRESSURE - MIN_PRESSURE));
			sprintf( color1, "rgb<%lf ,0 ,%lf>",
					1.-(MAX_PRESSURE - this->vesselSegments[s]->vesselNodes[1]->pressure)/(MAX_PRESSURE - MIN_PRESSURE),
					(MAX_PRESSURE - this->vesselSegments[s]->vesselNodes[1]->pressure)/(MAX_PRESSURE - MIN_PRESSURE));
			sprintf( color, "rgb<%lf ,0 ,%lf>",
					1.-(MAX_PRESSURE - 0.5*this->vesselSegments[s]->vesselNodes[0]->pressure - 0.5*this->vesselSegments[s]->vesselNodes[1]->pressure)/(MAX_PRESSURE - MIN_PRESSURE),
					(MAX_PRESSURE - 0.5*this->vesselSegments[s]->vesselNodes[0]->pressure - 0.5*this->vesselSegments[s]->vesselNodes[1]->pressure)/(MAX_PRESSURE - MIN_PRESSURE));
			break;
		}

		PovrayIO::writeCylinder( &fs,
				this->vesselSegments[s]->vesselNodes[0]->position[0],//+noise0[0],
				this->vesselSegments[s]->vesselNodes[0]->position[1],//+noise0[1],
				this->vesselSegments[s]->vesselNodes[0]->position[2],
				this->vesselSegments[s]->vesselNodes[1]->position[0],//+noise1[0],
				this->vesselSegments[s]->vesselNodes[1]->position[1],//+noise1[1],
				this->vesselSegments[s]->vesselNodes[1]->position[2],
				this->vesselSegments[s]->radius/LATTICE_CONSTANT,
				color);
				//(this->vesselSegments[s]->vesselNodes[1]->pressure+this->vesselSegments[s]->vesselNodes[1]->pressure!=0? "Red":"Blue"));
		PovrayIO::writeSphere( &fs,
				this->vesselSegments[s]->vesselNodes[0]->position[0],//+noise0[0],
				this->vesselSegments[s]->vesselNodes[0]->position[1],//+noise0[0],
				this->vesselSegments[s]->vesselNodes[0]->position[2],//+noise0[1],
				this->vesselSegments[s]->radius/LATTICE_CONSTANT,
				//(this->vesselSegments[s]->vesselNodes[0]->getType() == TIP||this->vesselSegments[s]->vesselNodes[0]->getType() == ROOT?.3:this->vesselSegments[s]->radius/10.),
				color0);
				//(this->vesselSegments[s]->vesselNodes[0]->pressure!=0?"Red":"Blue"));
		PovrayIO::writeSphere( &fs,
				this->vesselSegments[s]->vesselNodes[1]->position[0],//+noise1[0],
				this->vesselSegments[s]->vesselNodes[1]->position[1],//+noise1[0],
				this->vesselSegments[s]->vesselNodes[1]->position[2],//+noise1[1],
				this->vesselSegments[s]->radius/LATTICE_CONSTANT,
				//(this->vesselSegments[s]->vesselNodes[1]->getType() == TIP||this->vesselSegments[s]->vesselNodes[1]->getType() == ROOT?.3:this->vesselSegments[s]->radius/10.),
				color1);
				//(this->vesselSegments[s]->vesselNodes[1]->pressure!=0?"Red":"Blue"));

	}

	if(DOMAIN_SIZE_X==1)
	for( int z=0; z<DOMAIN_SIZE_Z; z++)
		for( int y=0; y<DOMAIN_SIZE_Y; y++)
			for( int x=0; x<DOMAIN_SIZE_X; x++)
			{
				/*char color[512];
				sprintf( color, "rgb<1, %lf ,%lf>", 1-marker[x][y][z], 1-marker[x][y][z]);
				fs << "polygon {";
				fs << "5,";
				fs << "<"<<x-0.5<<", "<<y-0.5<<">, <"<<x-0.5<<", "<<y+0.5<<">, <"<<x+0.5<<", "<<y+0.5<<">, <"<<x+0.5<<", "<<y-0.5<<">, <"<<x-0.5<<", "<<y-0.5<<">";
				fs << " pigment{color "<<color<<"} finish{ambient 1}}";
				 */
				char color[512];
				//sprintf( color, "rgbt<1,%lf,%lf,%lf>", 1-marker[x][y][z], 1-marker[x][y][z], 1-marker[x][y][z]);
				sprintf( color, "rgb<1, %lf ,%lf,%lf>", 1-marker[x][y][z], 1-marker[x][y][z], 0.7);
				PovrayIO::writeCube( &fs, x-0.5, y-0.5, z-0.5, x+0.5, y+0.5, z+0.5, color);
			}

	fs.close();
}


void VesselGraph::printToEPS(const char *filename, char mode, float ***marker)
{
	std::fstream fs;
	fs.open( filename, std::fstream::out);

	EpsIO::PSwriteHeader( &fs, 0, DOMAIN_SIZE_X, 0, DOMAIN_SIZE_Y);

	if( mode==1)
	for( int z=0; z<DOMAIN_SIZE_Z; z++)
		for( int y=0; y<DOMAIN_SIZE_Y; y++)
			for( int x=0; x<DOMAIN_SIZE_X; x++)
			{
				char color[512];
				EpsIO::PScolor( color, 1, 1-marker[x][y][z], 1-marker[x][y][z]);
				int N = 5;
				double X[] = {x, x,   x+1, x+1, x};
				double Y[] = {y, y+1, y+1, y, y};

				EpsIO::PSfillPolygon( &fs,
						X, Y, N,
						0., color);
			}

	for( int s=0; s<this->countVesselSegments; s++){
		//float noise0[2] = {RAND01,RAND01};
		//float noise1[2] = {RAND01,RAND01};

		char color[512],color0[512],color1[512];
		switch( mode){
		case 1:
			// Marker Concentration
			EpsIO::PScolor( color0, 1, 1-this->vesselSegments[s]->vesselNodes[0]->marker, 1-this->vesselSegments[s]->vesselNodes[0]->marker);
			EpsIO::PScolor( color1, 1, 1-this->vesselSegments[s]->vesselNodes[1]->marker, 1-this->vesselSegments[s]->vesselNodes[1]->marker);
			EpsIO::PScolor( color,  1, 1-this->vesselSegments[s]->vesselNodes[0]->marker*0.5-this->vesselSegments[s]->vesselNodes[1]->marker*0.5, 1-this->vesselSegments[s]->vesselNodes[0]->marker*0.5-this->vesselSegments[s]->vesselNodes[1]->marker*0.5);
			break;
		case 0:
			// Pressure
			EpsIO::PScolor( color0,
					1.-(MAX_PRESSURE - this->vesselSegments[s]->vesselNodes[0]->pressure)/(MAX_PRESSURE - MIN_PRESSURE),
					0,
					(MAX_PRESSURE - this->vesselSegments[s]->vesselNodes[0]->pressure)/(MAX_PRESSURE - MIN_PRESSURE));
			EpsIO::PScolor( color1,
					1.-(MAX_PRESSURE - this->vesselSegments[s]->vesselNodes[1]->pressure)/(MAX_PRESSURE - MIN_PRESSURE),
					0,
					(MAX_PRESSURE - this->vesselSegments[s]->vesselNodes[1]->pressure)/(MAX_PRESSURE - MIN_PRESSURE));
			EpsIO::PScolor( color,
					1.-(MAX_PRESSURE - 0.5*this->vesselSegments[s]->vesselNodes[0]->pressure - 0.5*this->vesselSegments[s]->vesselNodes[1]->pressure)/(MAX_PRESSURE - MIN_PRESSURE),
					0,
					(MAX_PRESSURE - 0.5*this->vesselSegments[s]->vesselNodes[0]->pressure - 0.5*this->vesselSegments[s]->vesselNodes[1]->pressure)/(MAX_PRESSURE - MIN_PRESSURE));
			break;
		}

		EpsIO::PSwriteLine( &fs,
				this->vesselSegments[s]->vesselNodes[0]->position[0]+0.5,//+noise0[0],
				this->vesselSegments[s]->vesselNodes[0]->position[1]+0.5,//+noise0[1],
				//this->vesselSegments[s]->vesselNodes[0]->position[2],
				this->vesselSegments[s]->vesselNodes[1]->position[0]+0.5,//+noise1[0],
				this->vesselSegments[s]->vesselNodes[1]->position[1]+0.5,//+noise1[1],
				//this->vesselSegments[s]->vesselNodes[1]->position[2],
				2*this->vesselSegments[s]->radius/LATTICE_CONSTANT,
				color);
				//(this->vesselSegments[s]->vesselNodes[1]->pressure+this->vesselSegments[s]->vesselNodes[1]->pressure!=0? "Red":"Blue"));
		EpsIO::PSwriteSphere( &fs,
				this->vesselSegments[s]->vesselNodes[0]->position[0]+0.5,//+noise0[0],
				this->vesselSegments[s]->vesselNodes[0]->position[1]+0.5,//+noise0[0],
				//this->vesselSegments[s]->vesselNodes[0]->position[2],//+noise0[1],
				this->vesselSegments[s]->radius/LATTICE_CONSTANT,
				//(this->vesselSegments[s]->vesselNodes[0]->getType() == TIP||this->vesselSegments[s]->vesselNodes[0]->getType() == ROOT?.3:this->vesselSegments[s]->radius/10.),
				color0);
				//(this->vesselSegments[s]->vesselNodes[0]->pressure!=0?"Red":"Blue"));
		EpsIO::PSwriteSphere( &fs,
				this->vesselSegments[s]->vesselNodes[1]->position[0]+0.5,//+noise1[0],
				this->vesselSegments[s]->vesselNodes[1]->position[1]+0.5,//+noise1[0],
				//this->vesselSegments[s]->vesselNodes[1]->position[2],//+noise1[1],
				this->vesselSegments[s]->radius/LATTICE_CONSTANT,
				//(this->vesselSegments[s]->vesselNodes[1]->getType() == TIP||this->vesselSegments[s]->vesselNodes[1]->getType() == ROOT?.3:this->vesselSegments[s]->radius/10.),
				color1);
				//(this->vesselSegments[s]->vesselNodes[1]->pressure!=0?"Red":"Blue"));

	}

	for( int vn=0; vn<this->countVesselNodes; vn++){

		char color[512];
		float radius=0;
		if( this->vesselNodes[vn]->getType() == ROOT)
				radius = 32./LATTICE_CONSTANT;
		if( this->vesselNodes[vn]->getType() == VESSEL)
				radius = 16./LATTICE_CONSTANT;
		if( this->vesselNodes[vn]->getType() == TIP)
				radius = 8./LATTICE_CONSTANT;

		EpsIO::PScolor( color,
				1.-(MAX_PRESSURE - 0.5*this->vesselNodes[vn]->pressure - 0.5*this->vesselNodes[vn]->pressure)/(MAX_PRESSURE - MIN_PRESSURE),
				0,
				(MAX_PRESSURE - 0.5*this->vesselNodes[vn]->pressure - 0.5*this->vesselNodes[vn]->pressure)/(MAX_PRESSURE - MIN_PRESSURE));

		EpsIO::PSwriteSphere( &fs,
				this->vesselNodes[vn]->position[0]+0.5,//+noise1[0],
				this->vesselNodes[vn]->position[1]+0.5,//+noise1[0],
				radius,
				color);

	}


	fs.close();
}


float VesselSegment::getViscosity()
{
	//return 4e-6 * (2.+exp(-this->radius/4.)*15.); // kPa * s

	return 4e-6 * (6 * exp(-0.085*this->radius*2.) + 3.1 - 2.44 * exp(-0.06*pow(this->radius*2., 0.645))); // kPa * s

	/*return
	- 0.93   *pow(log(this->radius), 7) 
	+ 12.42  *pow(log(this->radius), 6) 
	- 68.79  *pow(log(this->radius), 5)
	+ 205.42 *pow(log(this->radius), 4)
	- 358.38 *pow(log(this->radius), 3) 
	+ 367.3  *pow(log(this->radius), 2)
	- 204.1  *log(this->radius) 
	+ 48.4;*/
}


void VesselGraph::updateFlow()
{
#pragma omp parallel for
	for( int i=0; i<this->countVesselSegments; i++){
		// flow: n0 -> n1
		this->vesselSegments[i]->flow
			= PI/8. * pow( this->vesselSegments[i]->radius, 4)
			/ (this->vesselSegments[i]->getViscosity() * DISTANCE(this->vesselSegments[i]->vesselNodes[0], this->vesselSegments[i]->vesselNodes[1]) * LATTICE_CONSTANT)
			* ( this->vesselSegments[i]->vesselNodes[0]->pressure-this->vesselSegments[i]->vesselNodes[1]->pressure);
	}
}


void VesselGraph::updateShear()
{
#pragma omp parallel for
	for( int i=0; i<this->countVesselSegments; i++)
	//if( (this->vesselSegments[i]->vesselNodes[0]->getType() == VESSEL || this->vesselSegments[i]->vesselNodes[0]->getType() == ROOT) &&
	//	(this->vesselSegments[i]->vesselNodes[1]->getType() == VESSEL || this->vesselSegments[i]->vesselNodes[1]->getType() == ROOT))
	{
		this->vesselSegments[i]->shear 
			= 1./(2. * DISTANCE( this->vesselSegments[i]->vesselNodes[0], this->vesselSegments[i]->vesselNodes[1]) * LATTICE_CONSTANT)
			* this->vesselSegments[i]->radius
			* fabs(this->vesselSegments[i]->vesselNodes[0]->pressure-this->vesselSegments[i]->vesselNodes[1]->pressure);
	}
}


void VesselGraph::updatePressure( VesselNode **borderVesselNodes, float *borderPressure, int borderSize)
{
	// set border values
	for( int i=0; i<borderSize; i++)
		borderVesselNodes[i]->pressure = borderPressure[i];
		
	// iterative approximation
	float maxError = 0.;
	int its = 0;
	do{
		fprintf( stderr, "\rupdatePressure: %f \b", maxError);
		maxError = 0.;
		for( int i=0; i<this->countVesselNodes; i++)
			//if( this->vesselNodes[i]->countNeighbors>1)
			if( this->vesselNodes[i]->getType() == VESSEL || this->vesselNodes[i]->getType() == TIP)
			//if( this->vesselNodes[i]->type != ROOT)
			{
				/*float pressure = 0.;
				float count_neighbors = 0.;
				for( int v=0; v<this->vesselNodes[i]->countNeighbors; v++)
				if( this->vesselNodes[i]->neighbors[v]->getType() == VESSEL || this->vesselNodes[i]->neighbors[v]->getType() == ROOT || 
						this->vesselNodes[i]->neighbors[v]->getType() == TIP	){
					pressure += this->vesselNodes[i]->neighbors[v]->pressure;
					count_neighbors++;
				}
				//pressure /= (float)this->vesselNodes[i]->countNeighbors;
				if(count_neighbors==0)
					pressure = 0.;
				else
					pressure /= count_neighbors;
				if( maxError < fabs(pressure - this->vesselNodes[i]->pressure))
					maxError = fabs(pressure - this->vesselNodes[i]->pressure);
				this->vesselNodes[i]->pressure = pressure;
				*/
				float pressure = 0.;
				float pressure2 = 0.;
				float count_neighbors = 0.;
				for( int v=0; v<this->vesselNodes[i]->countNeighbors; v++)
				if( this->vesselNodes[i]->neighbors[v]->getType() == VESSEL || this->vesselNodes[i]->neighbors[v]->getType() == ROOT || 
						this->vesselNodes[i]->neighbors[v]->getType() == TIP	){
					//pressure += this->vesselNodes[i]->neighbors[v]->pressure;
					float G = pow( this->vesselNodes[i]->branches[v]->radius, 4) / (this->vesselNodes[i]->branches[v]->getViscosity() * DISTANCE(vesselNodes[i], vesselNodes[i]->neighbors[v]));

					pressure  += G*vesselNodes[i]->neighbors[v]->pressure;
					pressure2 += G;
					count_neighbors++;
				}
				//pressure /= (float)this->vesselNodes[i]->countNeighbors;
				if(count_neighbors==0)
					pressure = 0.;
				else
					pressure /= pressure2;
					//pressure /= count_neighbors;
				if( maxError < fabs(pressure - this->vesselNodes[i]->pressure))
					maxError = fabs(pressure - this->vesselNodes[i]->pressure);
				this->vesselNodes[i]->pressure = pressure;
			}
		its++;
	}while( maxError > 1e-10 && its < 1000);
	fprintf( stderr, "INFO: %i\n", its);
}

//SparseMatrix *sA=0;
//float *b=0;
//float *x=0;

typedef double pressure_t;

void VesselGraph::updatePressureNEW()
{
	pressure_t *b=0;
	pressure_t *x=0;

	//fprintf( stderr, "updatePressure\n");
	//if(sA==0)
	SparseMatrix<pressure_t> *sA = new SparseMatrix<pressure_t>(this->countVesselNodes,this->countVesselNodes);
	//if(b==0)
		b = (pressure_t*) malloc( sizeof(pressure_t) * this->countVesselNodes);
	//if(x==0)
		x = (pressure_t*) malloc( sizeof(pressure_t) * this->countVesselNodes);
	
	/*v0 = (float*) malloc( sizeof(float) * this->countVesselNodes);
	v1 = (float*) malloc( sizeof(float) * this->countVesselNodes);
	v2 = (float*) malloc( sizeof(float) * this->countVesselNodes);
	v3 = (float*) malloc( sizeof(float) * this->countVesselNodes);
	v4 = (float*) malloc( sizeof(float) * this->countVesselNodes);
	v5 = (float*) malloc( sizeof(float) * this->countVesselNodes);
	v6 = (float*) malloc( sizeof(float) * this->countVesselNodes);*/

	
	// INIT PRESSURE VALUES
	for( int i=0; i<this->countVesselNodes; i++){
		if( this->vesselNodes[i]->getType() == ROOT)
			x[i] = this->vesselNodes[i]->pressure;
		else
			x[i] = this->vesselNodes[i]->pressure;
			//x[i] = (MAX_PRESSURE+MIN_PRESSURE)/2.;
			//x[i] = RAND01;// * MAX_PRESSURE;
		if( i!=this->vesselNodes[i]->index){
			fprintf( stderr, "ERROR! Wrong index of node %i: %i\n", i, this->vesselNodes[i]->index);
			exit( 0);			
		}
	}
		
	// Construct Matrix
#pragma omp parallel for
	for( int i=0; i<this->countVesselNodes; i++){
		sA->resetRow(i);
		if( this->vesselNodes[i]->countNeighbors == 0)
			fprintf( stderr, "ERROR: Node without neighbors!\n");
		if( this->vesselNodes[i]->getType() == ROOT){
			// BOUNDARY: ROOT 
			sA->set(i,i, 1);
			b[i] = this->vesselNodes[i]->pressure;
		}else{
		//if( this->vesselNodes[i]->getType() == VESSEL || this->vesselNodes[i]->getType() == TIP){
			pressure_t sumG = 0.;
			pressure_t G;
			pressure_t count_neighbors = 0.;
			for( int v=0; v<this->vesselNodes[i]->countNeighbors; v++)
			/*if( this->vesselNodes[i]->neighbors[v]->getType() == VESSEL ||
				this->vesselNodes[i]->neighbors[v]->getType() == ROOT   || 
				this->vesselNodes[i]->neighbors[v]->getType() == TIP	)*/
			{
				G = /*PI/8. */ pow( this->vesselNodes[i]->branches[v]->radius, 4) / (this->vesselNodes[i]->branches[v]->getViscosity() * DISTANCE(vesselNodes[i], vesselNodes[i]->neighbors[v]) * LATTICE_CONSTANT);
				sumG += G;
				
				if(isnan(G)){
					fprintf( stderr, "ERROR!\n");
					exit( 0);
				}
				
				//sA->set(i,this->vesselNodes[i]->neighbors[v]->index, /*vesselNodes[i]->neighbors[v]->pressure*/G);
				//count_neighbors++;
			}
			//sA->set(i,i,sumG);
			
			for( int v=0; v<this->vesselNodes[i]->countNeighbors; v++)
			/*if( this->vesselNodes[i]->neighbors[v]->getType() == VESSEL ||
				this->vesselNodes[i]->neighbors[v]->getType() == ROOT   || 
				this->vesselNodes[i]->neighbors[v]->getType() == TIP	)*/
			{
				G = /*PI/8. */ pow( this->vesselNodes[i]->branches[v]->radius, 4) / (this->vesselNodes[i]->branches[v]->getViscosity() * DISTANCE(vesselNodes[i], vesselNodes[i]->neighbors[v]) * LATTICE_CONSTANT);

				if(isnan(G)){
					fprintf( stderr, "ERROR!\n");
					exit( 0);
				}
				if(sumG==0){
					fprintf( stderr, "ERROR: sumG=%lf!\n", sumG);
					exit( 0);
				}

				if(isnan(-G/sumG)){
					fprintf( stderr, "ERROR: G=%e, sumG=%e\n", G, sumG);
					exit( 0);
				}

				sA->set(i,this->vesselNodes[i]->neighbors[v]->index, -G/sumG);
				count_neighbors++;
			}
			
			sA->set(i,i,1.);//sumG);
			b[i] = 0.;
		}	
	}
		
	//SolveBiCGSTAB( sA, b, x, 20000, 1e-10);
	//ConjugateGradientSparse( sA, b, x, 20000, 1e-20);
	//sA->printMatrix( "A", "%10.3lf ");
	//fprintf( stderr, "Precondition System\n");

	Solver<pressure_t> *S = new Solver<pressure_t>( this->countVesselNodes, Solver<pressure_t>::BiCGSTAB );
	//S->maxit = 10000;
	//S->maxerr = 1e-8;
	/*S->PreconditionJacobi( sA, b);
	S->maxerr = 1e-8;
	S->maxit = 2000;
	fprintf( stderr, "Solve System\n");
	fprintf( stderr, "updatePressure finished after %i iterations\n", S->Solve( sA, b, x));
	 */

	/*SparseMatrix<pressure_t> *B = sA->copy();//new SparseMatrix<pressure_t>( this->countVesselNodes, this->countVesselNodes);
	S->IncompleteLUfactorization( B);
	pressure_t *temp = (pressure_t*) malloc( sizeof(pressure_t) * this->countVesselNodes);
	//S->SolveLowerTriangular( B, b, temp);
	//S->SolveUpperTriangular( B, temp, x);
	S->SolveLU( B,b,temp,x);*/
	fprintf( stderr, "updatePressure finished after %i iterations\n", S->Solve( sA, b, x));



	// NORM: ||A*x - b||
	pressure_t *Ax_b, *Ax;
	Ax = Ax_b = (pressure_t*) malloc( sizeof(pressure_t) * this->countVesselNodes);
	// A*x
	SparseMatrix<pressure_t>::sparseMatrixVectorProduct(sA,x, Ax);
	// A*x - b
	vectorDifference( Ax, b, Ax_b, this->countVesselNodes);
	// ||A*x - b||
	fprintf( stderr, "norm = %e\n", sqrt(dotProduct( Ax_b, Ax_b, this->countVesselNodes)));



	/*SparseMatrix<float> *B = new SparseMatrix<float>( this->countVesselNodes, this->countVesselNodes);
	S->IncompleteLUfactorization( sA, B);
	float *y = (float*) malloc( sizeof(float) * this->countVesselNodes);

	S->SolveLU( B,b,y,x);
	S->Solve( sA, b, x);
	 */

	for( int i=0; i<this->countVesselNodes; i++)
		if( this->vesselNodes[i]->getType() != ROOT)
			this->vesselNodes[i]->pressure = x[i];

	//fprintf( stderr, "updatePressure finished\n");
	delete S;
	delete sA;
	free(b);
	free(x);
}


void VesselGraph::updatePressureNEW2()
{
	// ALLOC MEMORY
	SparseMatrix<pressure_t> *sA = new SparseMatrix<pressure_t>(this->countVesselNodes,this->countVesselNodes);
	pressure_t *b = (pressure_t*) malloc( sizeof(pressure_t) * this->countVesselNodes);
	pressure_t *x = (pressure_t*) malloc( sizeof(pressure_t) * this->countVesselNodes);


	// INIT PRESSURE VALUES
	for( int i=0; i<this->countVesselNodes; i++){
		x[i] = this->vesselNodes[i]->pressure;

		if( i!=this->vesselNodes[i]->index){
			fprintf( stderr, "ERROR! Wrong index of node %i: %i\n", i, this->vesselNodes[i]->index);
			exit( 0);
		}
	}

	// CONSTRUCT LINEAR SYSTEM
#pragma omp parallel for
	for( int i=0; i<this->countVesselNodes; i++){

		sA->resetRow(i);

		if( this->vesselNodes[i]->countNeighbors == 0)
			fprintf( stderr, "ERROR: Node without neighbors!\n");


		if( this->vesselNodes[i]->getType() == ROOT){

			// BOUNDARY: ROOT
			sA->set(i,i, 1);
			b[i] = this->vesselNodes[i]->pressure;

		}else{

			pressure_t G, sumG = 0;

			for( int v=0; v<this->vesselNodes[i]->countNeighbors; v++)
			{
				G = PI/8. * pow( this->vesselNodes[i]->branches[v]->radius, 4) / (this->vesselNodes[i]->branches[v]->getViscosity() * DISTANCE(vesselNodes[i], vesselNodes[i]->neighbors[v]) * LATTICE_CONSTANT);
				sA->set(i,this->vesselNodes[i]->neighbors[v]->index, -G);

				sumG += G;
			}

			sA->set(i,i,sumG);
			b[i] = 0.;
		}
	}

	// SOLVE SYSTEM

	Solver<pressure_t> *S = new Solver<pressure_t>( this->countVesselNodes, Solver<pressure_t>::BiCGSTAB );
	S->PreconditionJacobi( sA, b);
	S->maxerr = 1e-8;
	S->maxit = 2000;
	fprintf( stderr, "Solve System\n");
	fprintf( stderr, "updatePressure finished after %i iterations\n", S->Solve( sA, b, x));


	/*SparseMatrix<pressure_t> *B = sA->copy();//new SparseMatrix<pressure_t>( this->countVesselNodes, this->countVesselNodes);
	S->IncompleteLUfactorization( B);
	pressure_t *temp = (pressure_t*) malloc( sizeof(pressure_t) * this->countVesselNodes);
	S->SolveLU( B,b,temp,x);
	//fprintf( stderr, "updatePressure finished after %i iterations\n", S->Solve( sA, b, x));*/



	// NORM: ||A*x - b||
	/*pressure_t *Ax_b, *Ax = Ax_b = (pressure_t*) malloc( sizeof(pressure_t) * this->countVesselNodes);
	// A*x
	SparseMatrix<pressure_t>::sparseMatrixVectorProduct(sA,x, Ax);
	// A*x - b
	vectorDifference( Ax, b, Ax_b, this->countVesselNodes);
	// ||A*x - b||
	fprintf( stderr, "norm = %e\n", sqrt(dotProduct( Ax_b, Ax_b, this->countVesselNodes)));*/


	for( int i=0; i<this->countVesselNodes; i++)
		if( this->vesselNodes[i]->getType() != ROOT)
			this->vesselNodes[i]->pressure = x[i];


	// FREE MEMORY

	delete S;
	delete sA;
	free(b);
	free(x);
}


void VesselGraph::updateMarkerVessels( float dt, float **markerVesselsIn, float **markerVesselsOut)
{
	float *b=0;
	float *x=0;

	//if(sA==0)
	SparseMatrix<float> *sA = new SparseMatrix<float>(this->countVesselNodes,this->countVesselNodes);
	//if(b==0)
	b = (float*) malloc( sizeof(float) * this->countVesselNodes);
	//if(x==0)
	x = (float*) malloc( sizeof(float) * this->countVesselNodes);

	/*v0 = (float*) malloc( sizeof(float) * this->countVesselNodes);
	v1 = (float*) malloc( sizeof(float) * this->countVesselNodes);
	v2 = (float*) malloc( sizeof(float) * this->countVesselNodes);
	v3 = (float*) malloc( sizeof(float) * this->countVesselNodes);
	v4 = (float*) malloc( sizeof(float) * this->countVesselNodes);
	v5 = (float*) malloc( sizeof(float) * this->countVesselNodes);
	v6 = (float*) malloc( sizeof(float) * this->countVesselNodes);
*/

	// INIT PRESSURE VALUES
	/*for( int x=0; x<DOMAIN_SIZE_X; x++)
		for( int y=0; y<DOMAIN_SIZE_Y; y++)
			markerVesselsOut[x][y] = 0.;
*/
	for( int i=0; i<this->countVesselNodes; i++){
		x[i] = this->vesselNodes[i]->marker;
	}

	// Construct Matrix
//#pragma omp parallel for
	for( int i=0; i<this->countVesselNodes; i++){
		if( this->vesselNodes[i]->countNeighbors == 0)
			fprintf( stderr, "ERROR: Node without neighbors!\n");
		if( this->vesselNodes[i]->getType() == ROOT){
			// BOUNDARY: ROOT
			sA->set(i,i, 1);
			if(this->vesselNodes[i]->pressure==MIN_PRESSURE)
				b[i] = 0;
			else
				b[i] = 1;
		}else{
			float dIN  = 0.;
			float dOUT = 0.;
			for( int v=0; v<this->vesselNodes[i]->countNeighbors; v++)
				// in flow
				if( this->vesselNodes[i]->pressure < this->vesselNodes[i]->neighbors[v]->pressure){
					dIN = fabs(this->vesselNodes[i]->branches[v]->flow) * dt; //* this->vesselNodes[i]->neighbors[v]->marker;
					sA->set(i,this->vesselNodes[i]->neighbors[v]->index, -dIN);
				}
				else
					dOUT+= fabs(this->vesselNodes[i]->branches[v]->flow) * dt; //* this->vesselNodes[i]->marker;

			sA->set(i,i, 1+dOUT);
			b[i] = this->vesselNodes[i]->marker;
		}
	}

	//SolveBiCGSTAB( sA, b, x, 20000, 1e-10);
	//ConjugateGradientSparse( sA, b, x, 20000, 1e-20);
	Solver<float> *S = new Solver<float>( this->countVesselNodes, Solver<float>::BiCGSTAB);
	S->Solve( sA, b, x);
	delete S;

	for( int i=0; i<this->countVesselNodes; i++)
		this->vesselNodes[i]->marker = x[i];


	delete sA;
	free(b);
	free(x);
}


void VesselGraph::updateMarkerVesselsAndInterstitialSpace( float dt, float **markerVesselsIn, float **markerVesselsOut, float ***markerIntSpaceIn, float ***markerIntSpaceOut)
{
	float *b=0;
	float *x=0;

	int M = this->countVesselNodes;
	int N = DOMAIN_SIZE_X*DOMAIN_SIZE_Y*DOMAIN_SIZE_Z;
	//if(sA==0)
	SparseMatrix<float> *sA = new SparseMatrix<float>(M+N,M+N);
	//if(b==0)
	b = (float*) malloc( sizeof(float) * (M+N));
	//if(x==0)
	x = (float*) malloc( sizeof(float) * (M+N));

	/*v0 = (float*) malloc( sizeof(float) * (M+N));
	v1 = (float*) malloc( sizeof(float) * (M+N));
	v2 = (float*) malloc( sizeof(float) * (M+N));
	v3 = (float*) malloc( sizeof(float) * (M+N));
	v4 = (float*) malloc( sizeof(float) * (M+N));
	v5 = (float*) malloc( sizeof(float) * (M+N));
	v6 = (float*) malloc( sizeof(float) * (M+N));
	 */

	// INIT PRESSURE VALUES
	for( int i=0; i<M; i++){
		x[i] = this->vesselNodes[i]->marker;
	}

	// Construct Matrix for Vascular Marker Concentration
	float volume = 0.5;
//#pragma omp parallel for
	for( int i=0; i<M; i++){
		if( this->vesselNodes[i]->countNeighbors == 0)
			fprintf( stderr, "ERROR: Node without neighbors!\n");
		if( this->vesselNodes[i]->getType() == ROOT && this->vesselNodes[i]->pressure==MAX_PRESSURE){
			// BOUNDARY: ROOT
			sA->set(i,i, 1);
			b[i] = ARTERIAL_MARKER_CONC;
		}else{
			float r=volume/dt + EXCHANGE_RATE_VESSELS_INTERSTITIAL;
			float sumFlow=0.;
			
			// c_P^n+1
			sA->set(i,i, 1);
			
			// c_P,i^n+1 (for all neighbors i)
			for( int v=0; v<this->vesselNodes[i]->countNeighbors; v++)
			{
				float flow;
				// in flow
				if( this->vesselNodes[i]->pressure < this->vesselNodes[i]->neighbors[v]->pressure){
					flow = fabs(this->vesselNodes[i]->branches[v]->flow);
				}
				// out flow
				else{
					flow = -fabs(this->vesselNodes[i]->branches[v]->flow);
				}
				sumFlow += flow;
				sA->set(i,this->vesselNodes[i]->neighbors[v]->index, flow/r);
			}
			
			// c_I^n+1
			int index = this->vesselNodes[i]->position[0] 
			          + this->vesselNodes[i]->position[1]*DOMAIN_SIZE_X 
			          + this->vesselNodes[i]->position[2]*DOMAIN_SIZE_X*DOMAIN_SIZE_Y;
			sA->set(i,M+index, -EXCHANGE_RATE_VESSELS_INTERSTITIAL/r);
			
			// b
			b[i] = ARTERIAL_MARKER_CONC*sumFlow/r + volume/(dt*r) * this->vesselNodes[i]->marker;
		}
	}

	// Construct Matrix for Interstitial Marker Concentration
//#pragma omp parallel for
	for( int z=0; z<DOMAIN_SIZE_Z; z++)
		for( int y=0; y<DOMAIN_SIZE_Y; y++)
			for( int x=0; x<DOMAIN_SIZE_X; x++)
			{
				int i = M
					  + x 
				      + y*DOMAIN_SIZE_X 
				      + z*DOMAIN_SIZE_X*DOMAIN_SIZE_Y;
				
				// c_I^n+1
				sA->set(i,i, 1);
				
				// r
				float r=(1.-volume)/dt + EXCHANGE_RATE_VESSELS_INTERSTITIAL;
				for( int nx=(x>0?x-1:x+1); nx<=x+1 && nx<DOMAIN_SIZE_X; nx+=2)
					r+=MARKER_DIFFUSION_COEFFICIENT;
				for( int ny=(y>0?y-1:y+1); ny<=y+1 && ny<DOMAIN_SIZE_Y; ny+=2)
					r+=MARKER_DIFFUSION_COEFFICIENT;
				for( int nz=(z>0?z-1:z+1); nz<=z+1 && nz<DOMAIN_SIZE_Z; nz+=2)
					r+=MARKER_DIFFUSION_COEFFICIENT;
				
				// c_I,i^n+1 (for all neighbors i)
				for( int nx=(x>0?x-1:x+1); nx<=x+1 && nx<DOMAIN_SIZE_X; nx+=2)
				{
					int ii = M
						   + nx
					       + y*DOMAIN_SIZE_X
					       + z*DOMAIN_SIZE_X*DOMAIN_SIZE_Y;
					sA->set(i,ii, -MARKER_DIFFUSION_COEFFICIENT/r);
				}
				for( int ny=(y>0?y-1:y+1); ny<=y+1 && ny<DOMAIN_SIZE_Y; ny+=2)
				{
					int ii = M
						   + x
					       + ny*DOMAIN_SIZE_X
					       + z*DOMAIN_SIZE_X*DOMAIN_SIZE_Y;
					sA->set(i,ii, -MARKER_DIFFUSION_COEFFICIENT/r);
				}
				for( int nz=(z>0?z-1:z+1); nz<=z+1 && nz<DOMAIN_SIZE_Z; nz+=2)
				{
					int ii = M
						   + x
					       + y*DOMAIN_SIZE_X
					       + nz*DOMAIN_SIZE_X*DOMAIN_SIZE_Y;
					sA->set(i,ii, -MARKER_DIFFUSION_COEFFICIENT/r);
				}
				
				// c_P^n+1
				VesselNode *vn = this->octree->at( x,y,z);
				if( vn!=0)
					sA->set(i,vn->index, -EXCHANGE_RATE_VESSELS_INTERSTITIAL/r);

				// b
				b[i] = (1.-volume)/(dt*r) * markerIntSpaceIn[x][y][z];
			}

				
	/*for( int i=0; i<this->countVesselNodes; i++){
		if( this->vesselNodes[i]->countNeighbors == 0)
			fprintf( stderr, "ERROR: Node without neighbors!\n");
		if( this->vesselNodes[i]->getType() == ROOT && this->vesselNodes[i]->pressure==MAX_PRESSURE){
			// BOUNDARY: ROOT
			b[i] = ARTERIAL_MARKER_CONC;
		}else{
			float dIN  = 0.;
			float dOUT = 0.;
			for( int v=0; v<this->vesselNodes[i]->countNeighbors; v++)
				// in flow
				if( this->vesselNodes[i]->pressure < this->vesselNodes[i]->neighbors[v]->pressure){
					dIN = fabs(this->vesselNodes[i]->branches[v]->flow) * dt; // * this->vesselNodes[i]->neighbors[v]->marker;
					sA->set(i,this->vesselNodes[i]->neighbors[v]->index, -dIN);
				}
				else
					dOUT+= fabs(this->vesselNodes[i]->branches[v]->flow) * dt; // * this->vesselNodes[i]->marker;

			sA->set(i,i, 1+dOUT);
			b[i] = this->vesselNodes[i]->marker;
		}
	}*/

	//SolveBiCGSTAB( sA, b, x, 20000, 1e-10);
	//ConjugateGradientSparse( sA, b, x, 20000, 1e-20);
	Solver<float> *S = new Solver<float>( M+N, Solver<float>::BiCGSTAB);
	S->Solve( sA, b, x);
	delete S;

	for( int i=0; i<M; i++)
		this->vesselNodes[i]->marker = x[i];
	for( int iz=0; iz<DOMAIN_SIZE_Z; iz++)
		for( int iy=0; iy<DOMAIN_SIZE_Y; iy++)
			for( int ix=0; ix<DOMAIN_SIZE_X; ix++)
			{
				int i = M
					  + ix
				      + iy*DOMAIN_SIZE_X
				      + iz*DOMAIN_SIZE_X*DOMAIN_SIZE_Y;
				markerIntSpaceOut[ix][iy][iz] = x[i];
			}


	delete sA;
	free(b);
	free(x);
}


void VesselGraph::updateMarkerVesselsAndInterstitialSpaceNEW( float dt, float **markerVesselsIn, float **markerVesselsOut, float ***markerIntSpaceIn, float ***markerIntSpaceOut)
{
	float *b=0;
	float *x=0;

	int M = this->countVesselNodes;
	int N = DOMAIN_SIZE_X*DOMAIN_SIZE_Y*DOMAIN_SIZE_Z;
	//if(sA==0)
	SparseMatrix<float> *sA = new SparseMatrix<float>(M+N,M+N);
	//if(b==0)
	b = (float*) malloc( sizeof(float) * (M+N));
	//if(x==0)
	x = (float*) malloc( sizeof(float) * (M+N));

	/*v0 = (float*) malloc( sizeof(float) * (M+N));
	v1 = (float*) malloc( sizeof(float) * (M+N));
	v2 = (float*) malloc( sizeof(float) * (M+N));
	v3 = (float*) malloc( sizeof(float) * (M+N));
	v4 = (float*) malloc( sizeof(float) * (M+N));
	v5 = (float*) malloc( sizeof(float) * (M+N));
	v6 = (float*) malloc( sizeof(float) * (M+N));
	 */
 
	// INIT PRESSURE VALUES
	for( int i=0; i<M; i++){
		x[i] = this->vesselNodes[i]->marker;
	}

	// Construct Matrix for Vascular Marker Concentration
	//float volume = 0.5;
//#pragma omp parallel for
	for( int i=0; i<M; i++){
		if( this->vesselNodes[i]->countNeighbors == 0)
			fprintf( stderr, "ERROR: Node without neighbors!\n");
		if( this->vesselNodes[i]->getType() == ROOT){
			// BOUNDARY: ROOT
			sA->set(i,i, 1);
			if( this->vesselNodes[i]->pressure==MAX_PRESSURE)
				b[i] = ARTERIAL_MARKER_CONC;
			else
				b[i] = 0;
		}else{
			//float r=volume/dt + EXCHANGE_RATE_VESSELS_INTERSTITIAL;
			//float sumFlow=0.;
			//float volume = 1.;

			// c_P^n+1
			sA->set(i,i, 1);

			// r
			float r = 0.;
			for( int v=0; v<this->vesselNodes[i]->countNeighbors; v++)
			{
				// out flow
				if( this->vesselNodes[i]->pressure >= this->vesselNodes[i]->neighbors[v]->pressure)
					r += fabs(this->vesselNodes[i]->branches[v]->flow);
			}
			r = 1. + dt*r + dt*EXCHANGE_RATE_VESSELS_INTERSTITIAL;

			// c_P,i^n+1 (for all neighbors i)
			for( int v=0; v<this->vesselNodes[i]->countNeighbors; v++)
			{
				float flow;
				// in flow
				if( this->vesselNodes[i]->pressure < this->vesselNodes[i]->neighbors[v]->pressure){
					flow = -fabs(this->vesselNodes[i]->branches[v]->flow);
					sA->set(i,this->vesselNodes[i]->neighbors[v]->index, flow*dt/r);
				}
			}

			// c_I^n+1
			int index = this->vesselNodes[i]->position[0]
			          + this->vesselNodes[i]->position[1]*DOMAIN_SIZE_X
			          + this->vesselNodes[i]->position[2]*DOMAIN_SIZE_X*DOMAIN_SIZE_Y;
			sA->set(i,M+index, -EXCHANGE_RATE_VESSELS_INTERSTITIAL*dt/r);

			// b
			b[i] = 1./r * this->vesselNodes[i]->marker;
		}
	}

	// Construct Matrix for Interstitial Marker Concentration
//#pragma omp parallel for
	for( int z=0; z<DOMAIN_SIZE_Z; z++)
		for( int y=0; y<DOMAIN_SIZE_Y; y++)
			for( int x=0; x<DOMAIN_SIZE_X; x++)
			{
				int i = M
					  + x
				      + y*DOMAIN_SIZE_X
				      + z*DOMAIN_SIZE_X*DOMAIN_SIZE_Y;

				// c_I^n+1
				sA->set(i,i, 1);

				// r
				//float r=(1.-volume)/dt + EXCHANGE_RATE_VESSELS_INTERSTITIAL;
				float r = 0.;
				for( int nx=(x>0?x-1:x+1); nx<=x+1 && nx<DOMAIN_SIZE_X; nx+=2)
					r+=1;
				for( int ny=(y>0?y-1:y+1); ny<=y+1 && ny<DOMAIN_SIZE_Y; ny+=2)
					r+=1;
				for( int nz=(z>0?z-1:z+1); nz<=z+1 && nz<DOMAIN_SIZE_Z; nz+=2)
					r+=1;
				r = 1. + dt*MARKER_DIFFUSION_COEFFICIENT*r + dt*EXCHANGE_RATE_VESSELS_INTERSTITIAL;

				// c_I,i^n+1 (for all neighbors i)
				for( int nx=(x>0?x-1:x+1); nx<=x+1 && nx<DOMAIN_SIZE_X; nx+=2)
				{
					int ii = M
						   + nx
					       + y*DOMAIN_SIZE_X
					       + z*DOMAIN_SIZE_X*DOMAIN_SIZE_Y;
					sA->set(i,ii, -MARKER_DIFFUSION_COEFFICIENT*dt/r);
				}
				for( int ny=(y>0?y-1:y+1); ny<=y+1 && ny<DOMAIN_SIZE_Y; ny+=2)
				{
					int ii = M
						   + x
					       + ny*DOMAIN_SIZE_X
					       + z*DOMAIN_SIZE_X*DOMAIN_SIZE_Y;
					sA->set(i,ii, -MARKER_DIFFUSION_COEFFICIENT*dt/r);
				}
				for( int nz=(z>0?z-1:z+1); nz<=z+1 && nz<DOMAIN_SIZE_Z; nz+=2)
				{
					int ii = M
						   + x
					       + y*DOMAIN_SIZE_X
					       + nz*DOMAIN_SIZE_X*DOMAIN_SIZE_Y;
					sA->set(i,ii, -MARKER_DIFFUSION_COEFFICIENT*dt/r);
				}

				// c_P^n+1
				VesselNode *vn = this->octree->at( x,y,z);
				if( vn!=0)
					sA->set(i,vn->index, -EXCHANGE_RATE_VESSELS_INTERSTITIAL*dt/r);

				// b
				b[i] = 1./r * markerIntSpaceIn[x][y][z];
			}


	//SolveBiCGSTAB( sA, b, x, 20000, 1e-10);
	//ConjugateGradientSparse( sA, b, x, 20000, 1e-20);
	Solver<float> *S = new Solver<float>( M+N, Solver<float>::BiCGSTAB);
	S->Solve( sA, b, x);
	delete S;

	for( int i=0; i<M; i++)
		this->vesselNodes[i]->marker = x[i];
	for( int iz=0; iz<DOMAIN_SIZE_Z; iz++)
		for( int iy=0; iy<DOMAIN_SIZE_Y; iy++)
			for( int ix=0; ix<DOMAIN_SIZE_X; ix++)
			{
				int i = M
					  + ix
				      + iy*DOMAIN_SIZE_X
				      + iz*DOMAIN_SIZE_X*DOMAIN_SIZE_Y;
				markerIntSpaceOut[ix][iy][iz] = x[i];
			}


	delete sA;
	free(b);
	free(x);
}


void VesselGraph::updateMarkerVesselsAndInterstitialSpaceNEW2( float dt, float border, float ***markerIntSpaceIn, float ***markerIntSpaceOut,
		SparseMatrix<float> *&sA, float *&b, float *&x)
{
	int M = this->countVesselNodes;
	int N = DOMAIN_SIZE_X*DOMAIN_SIZE_Y*DOMAIN_SIZE_Z;
	if(sA==0)
	/*SparseMatrix<float> */sA = new SparseMatrix<float>(M+N,M+N);
	if(b==0)
	b = (float*) malloc( sizeof(float) * (M+N));
	if(x==0)
	x = (float*) malloc( sizeof(float) * (M+N));

	/*v0 = (float*) malloc( sizeof(float) * (M+N));
	v1 = (float*) malloc( sizeof(float) * (M+N));
	v2 = (float*) malloc( sizeof(float) * (M+N));
	v3 = (float*) malloc( sizeof(float) * (M+N));
	v4 = (float*) malloc( sizeof(float) * (M+N));
	v5 = (float*) malloc( sizeof(float) * (M+N));
	v6 = (float*) malloc( sizeof(float) * (M+N));*/
	Solver<float> *S = new Solver<float>( M+N, Solver<float>::BiCGSTAB );

 
	// INIT MARKER CONC.
	for( int i=0; i<M; i++){
		x[i] = this->vesselNodes[i]->marker;
		sA->resetRow(i);
	}
	for( int i=M; i<M+N; i++){
		x[i] = 0;
		sA->resetRow(i);
	}

	// Construct Matrix for Vascular Marker Concentration
	//float volume = 0.5;
//#pragma omp parallel for
	for( int i=0; i<M; i++){
		if( this->vesselNodes[i]->countNeighbors == 0)
			fprintf( stderr, "ERROR: Node without neighbors!\n");
		if( this->vesselNodes[i]->getType() == ROOT && this->vesselNodes[i]->pressure==MAX_PRESSURE){
			// BOUNDARY: ROOT
			//fprintf( stderr, "Found Root with pressure=%f\n", this->vesselNodes[i]->pressure);
			sA->set(i,i, 1);
			if( this->vesselNodes[i]->pressure==MAX_PRESSURE){
				//b[i] = ARTERIAL_MARKER_CONC;
				b[i] = border;
			}
			else
				b[i] = 0;
		}else{
			//float r=volume/dt + EXCHANGE_RATE_VESSELS_INTERSTITIAL;
			//float sumFlow=0.;
			
			// vessel volume
			float volume = 0.;
			for( int v=0; v<this->vesselNodes[i]->countNeighbors; v++)
				volume += 0.5*LATTICE_CONSTANT
						* PI * this->vesselNodes[i]->branches[v]->radius*this->vesselNodes[i]->branches[v]->radius;
			

			// c_P^n+1
			sA->set(i,i, 1);

			// r
			float r = 0.;
			for( int v=0; v<this->vesselNodes[i]->countNeighbors; v++)
			{
				// out flow
				//if( this->vesselNodes[i]->pressure >= this->vesselNodes[i]->neighbors[v]->pressure)
				//	r += fabs(this->vesselNodes[i]->branches[v]->flow);

				// in flow
				if( this->vesselNodes[i]->pressure < this->vesselNodes[i]->neighbors[v]->pressure)
					r += fabs(this->vesselNodes[i]->branches[v]->flow);
			}
			r = 1. + dt/volume*r + dt/volume*EXCHANGE_RATE_VESSELS_INTERSTITIAL;

			// c_P,i^n+1 (for all neighbors i)
			for( int v=0; v<this->vesselNodes[i]->countNeighbors; v++)
			{
				float flow;
				// in flow
				if( this->vesselNodes[i]->pressure < this->vesselNodes[i]->neighbors[v]->pressure){
					flow = -fabs(this->vesselNodes[i]->branches[v]->flow);
					sA->set(i,this->vesselNodes[i]->neighbors[v]->index, flow*dt/r/volume);
					if( isnan(flow*dt/r/volume)){
						fprintf( stderr, "flow=%lf dt=%lf r=%lf volume=%lf\n",flow,dt,r,volume);
						exit(0);
					}
				}
			}

			// c_I^n+1
			int index = this->vesselNodes[i]->position[0]
			          + this->vesselNodes[i]->position[1]*DOMAIN_SIZE_X
			          + this->vesselNodes[i]->position[2]*DOMAIN_SIZE_X*DOMAIN_SIZE_Y;
			sA->set(i,M+index, -EXCHANGE_RATE_VESSELS_INTERSTITIAL*dt/r/volume);
			if( isnan(EXCHANGE_RATE_VESSELS_INTERSTITIAL*dt/r/volume)){
				fprintf( stderr, "EXCHANGE_RATE_VESSELS_INTERSTITIAL=%lf dt=%lf r=%lf volume=%lf\n",EXCHANGE_RATE_VESSELS_INTERSTITIAL,dt,r,volume);
				exit(0);
			}

			// b
			b[i] = 1./r * this->vesselNodes[i]->marker;
			if( isnan(b[i])){
				fprintf( stderr, "r=%lf this->vesselNodes[i]->marker=%lf\n",r,this->vesselNodes[i]->marker);
				exit(0);
			}

		}
	}

	// Construct Matrix for Interstitial Marker Concentration
//#pragma omp parallel for
	for( int z=0; z<DOMAIN_SIZE_Z; z++)
		for( int y=0; y<DOMAIN_SIZE_Y; y++)
			for( int x=0; x<DOMAIN_SIZE_X; x++)
			{
				int i = M
					  + x
				      + y*DOMAIN_SIZE_X
				      + z*DOMAIN_SIZE_X*DOMAIN_SIZE_Y;


				// volume: tissue
				VesselNode *vn = this->octree->at( x,y,z);
				float volume = LATTICE_CONSTANT*LATTICE_CONSTANT*LATTICE_CONSTANT;
				if( vn!=0)
				for( int v=0; v<vn->countNeighbors; v++)
					volume -= 0.5*LATTICE_CONSTANT
						* PI * vn->branches[v]->radius*vn->branches[v]->radius;
				
				if(volume < 0.){
					fprintf( stderr, "ERROR: negative volume: %lf!!\n", volume);
					fprintf( stderr, "ERROR: max volume: %lf = %lf^3\n", (float)LATTICE_CONSTANT*LATTICE_CONSTANT*LATTICE_CONSTANT, (float)LATTICE_CONSTANT);
					for( int v=0; v<vn->countNeighbors; v++)
						fprintf( stderr, "%i neighbor: radius=%lf, volume=%lf\n", v, vn->branches[v]->radius, 0.5*LATTICE_CONSTANT	* PI * vn->branches[v]->radius*vn->branches[v]->radius);
					exit(0);
				}
						
				// c_I^n+1
				sA->set(i,i, 1);

				// r
				//float r=(1.-volume)/dt + EXCHANGE_RATE_VESSELS_INTERSTITIAL;
				float r = 0.;
				for( int nx=(x>0?x-1:x+1); nx<=x+1 && nx<DOMAIN_SIZE_X; nx+=2)
					r+=1;
				for( int ny=(y>0?y-1:y+1); ny<=y+1 && ny<DOMAIN_SIZE_Y; ny+=2)
					r+=1;
				for( int nz=(z>0?z-1:z+1); nz<=z+1 && nz<DOMAIN_SIZE_Z; nz+=2)
					r+=1;
				r = 1. 
				  + dt * MARKER_DIFFUSION_COEFFICIENT/(LATTICE_CONSTANT*LATTICE_CONSTANT) * r 
				  + dt * EXCHANGE_RATE_VESSELS_INTERSTITIAL/volume;

				// c_I,i^n+1 (for all neighbors i)
				for( int nx=(x>0?x-1:x+1); nx<=x+1 && nx<DOMAIN_SIZE_X; nx+=2)
				{
					int ii = M
						   + nx
					       + y*DOMAIN_SIZE_X
					       + z*DOMAIN_SIZE_X*DOMAIN_SIZE_Y;
					sA->set(i,ii, -MARKER_DIFFUSION_COEFFICIENT/(LATTICE_CONSTANT*LATTICE_CONSTANT) * dt/r);
					if( isnan(MARKER_DIFFUSION_COEFFICIENT/(LATTICE_CONSTANT*LATTICE_CONSTANT) * dt/r)){
						fprintf( stderr, "r=%lf\n",r);
						exit(0);
					}
				}
				for( int ny=(y>0?y-1:y+1); ny<=y+1 && ny<DOMAIN_SIZE_Y; ny+=2)
				{
					int ii = M
						   + x
					       + ny*DOMAIN_SIZE_X
					       + z*DOMAIN_SIZE_X*DOMAIN_SIZE_Y;
					sA->set(i,ii, -MARKER_DIFFUSION_COEFFICIENT/(LATTICE_CONSTANT*LATTICE_CONSTANT) * dt/r);
					if( isnan(MARKER_DIFFUSION_COEFFICIENT/(LATTICE_CONSTANT*LATTICE_CONSTANT) * dt/r)){
						fprintf( stderr, "r=%lf\n",r);
						exit(0);
					}
				}
				for( int nz=(z>0?z-1:z+1); nz<=z+1 && nz<DOMAIN_SIZE_Z; nz+=2)
				{
					int ii = M
						   + x
					       + y*DOMAIN_SIZE_X
					       + nz*DOMAIN_SIZE_X*DOMAIN_SIZE_Y;
					sA->set(i,ii, -MARKER_DIFFUSION_COEFFICIENT/(LATTICE_CONSTANT*LATTICE_CONSTANT) * dt/r);
					if( isnan(MARKER_DIFFUSION_COEFFICIENT/(LATTICE_CONSTANT*LATTICE_CONSTANT) * dt/r)){
						fprintf( stderr, "r=%lf\n",r);
						exit(0);
					}
				}

				// c_P^n+1
				if( vn!=0){
					sA->set(i,vn->index, -EXCHANGE_RATE_VESSELS_INTERSTITIAL/volume * dt/r);
					if( isnan(-EXCHANGE_RATE_VESSELS_INTERSTITIAL/volume * dt/r)){
						fprintf( stderr, "r=%lf volume=%lf\n",r,volume);
						exit(0);
					}
				}

				// b
				b[i] = 1./r * markerIntSpaceIn[x][y][z];
				if( isnan(b[i])){
					fprintf( stderr, "r=%lf markerIntSpaceIn[x][y][z]=%lf\n",r,markerIntSpaceIn[x][y][z]);
					exit(0);
				}
			}


	//SolveBiCGSTAB( sA, b, x, 20000, 1e-10);

	//ConjugateGradientSparse( sA, b, x, 20000, 1e-20);
	//fprintf( stderr, "Precondition System\n");

	//S->PreconditionJacobi( sA, b);
	//S->maxerr = 1e-6;
	//fprintf( stderr, "Solve System\n");
	if( S->Solve( sA, b, x) == 0){
		fprintf(stderr, "[AIF=%f] ", border);
		printVector( b, sA->dimI, "b", "%10.3f");
		//sA->printMatrix( "A", "%10.3f");
		//exit(0);
	}

	/*S->IncompleteLUfactorization( sA);
	float *y = (float*) malloc( sizeof(float) * (M+N));
	S->SolveLU( sA, b, y, x);
	free(y);*/
	delete S;

	for( int i=0; i<M; i++){
		this->vesselNodes[i]->marker = x[i];
		if( isnan(x[i]) ){
			fprintf( stderr, "x[%i] = %lf\n", i, x[i]);
			exit(0);
		}
	}

	for( int iz=0; iz<DOMAIN_SIZE_Z; iz++)
		for( int iy=0; iy<DOMAIN_SIZE_Y; iy++)
			for( int ix=0; ix<DOMAIN_SIZE_X; ix++)
			{
				int i = M
					  + ix
				      + iy*DOMAIN_SIZE_X
				      + iz*DOMAIN_SIZE_X*DOMAIN_SIZE_Y;
				markerIntSpaceOut[ix][iy][iz] = x[i];
				if( isnan(x[i]) ){
					fprintf( stderr, "x[%i] = %lf\n", i, x[i]);
					exit(0);
				}
			}


	/*delete sA;
	free(b);
	free(x);*/
}


void VesselGraph::updateMarkerVesselsExplicit( float dt, float border, float *&dx)
{
	int M = this->countVesselNodes;
	if(dx==0)
	dx = (float*) malloc( sizeof(float) * (M));


	// INIT MARKER CONC.
	for( int i=0; i<M; i++){
		dx[i] = this->vesselNodes[i]->marker;
	}


	// Construct Matrix for Vascular Marker Concentration
	for( int i=0; i<M; i++){
		if( this->vesselNodes[i]->getType() == ROOT /*&& this->vesselNodes[i]->pressure==MAX_PRESSURE*/){

			// BOUNDARY: ROOT
			//fprintf( stderr, "Found Root with pressure=%f\n", this->vesselNodes[i]->pressure);

			if( this->vesselNodes[i]->pressure==MAX_PRESSURE)
				dx[i] = (border - this->vesselNodes[i]->marker) / dt;
			else
				dx[i] = 0;
		}else{

			dx[i] = 0;
			float outFlow=0, inFlow=0, diffusion=0;

			switch( 0){

			case 0: // 3-POINT EXPLICIT WITH NUMERICAL DIFFUSION (SECOND ORDER)
			{
				float flow = 0.;

				for( int v=0; v<this->vesselNodes[i]->countNeighbors; v++)
				{
					float courantNumber = this->vesselNodes[i]->getFlowFromNeighbor(v) * dt / this->getVascularVolume( this->vesselNodes[i]);
					float diffusionCoefficient = fabs(courantNumber) / 2.;

					// flow
					flow += courantNumber * (this->vesselNodes[i]->neighbors[v]->marker + this->vesselNodes[i]->marker)/2.;

					// "numerical" diffusion
					diffusion += diffusionCoefficient * (this->vesselNodes[i]->neighbors[v]->marker - this->vesselNodes[i]->marker);
				}

				dx[i] = flow + diffusion;
				break;
			}

			case 1:	// EXPLICIT UPWIND (FIRST ORDER UPWIND)
			{
				float flow = 0.;

				for( int v=0; v<this->vesselNodes[i]->countNeighbors; v++)
				{
					// in flow
					if (this->vesselNodes[i]->pressure < this->vesselNodes[i]->neighbors[v]->pressure) {

						float courantNumber = this->vesselNodes[i]->getFlowFromNeighbor(v) * dt / this->getVascularVolume( this->vesselNodes[i]);
						flow += courantNumber
								* (this->vesselNodes[i]->neighbors[v]->marker - this->vesselNodes[i]->marker);
					}
				}

				dx[i] = flow;
				break;
			}

			case 2:	// FIRST ORDER LAX-FRIEDRICHS
			{
				/*for( int v=0; v<this->vesselNodes[i]->countNeighbors; v++)
				{
						// out flow
						if( this->vesselNodes[i]->pressure >= this->vesselNodes[i]->neighbors[v]->pressure){

							outFlow	+= fabs(this->vesselNodes[i]->branches[v]->flow) / this->getVascularVolume( this->vesselNodes[i])
									* this->vesselNodes[i]->neighbors[v]->marker;
						}

						// in flow
						if (this->vesselNodes[i]->pressure < this->vesselNodes[i]->neighbors[v]->pressure) {

							inFlow += fabs(this->vesselNodes[i]->branches[v]->flow) / this->getVascularVolume( this->vesselNodes[i])
									* this->vesselNodes[i]->neighbors[v]->marker;
						}

						//weight = 1.;//fabs(this->vesselNodes[i]->branches[v]->flow) / this->getVascularVolume( this->vesselNodes[i]) * dt;
						//weightSum += weight;
						//neighborConc += weight * (this->vesselNodes[i]->neighbors[v]->marker - this->vesselNodes[i]->marker);
				}*/
				float flow=0.;
				for( int v=0; v<this->vesselNodes[i]->countNeighbors; v++){
					float courantNumber = this->vesselNodes[i]->getFlowFromNeighbor(v) * dt / this->getVascularVolume( this->vesselNodes[i]);
					flow += (courantNumber+1.)*vesselNodes[i]->neighbors[v]->marker;
				}

				//dx[i] = dt * (inFlow - outFlow + diffusion) + neighborConc/weightSum;
				dx[i] = - this->vesselNodes[i]->marker
						//+ (this->vesselNodes[i]->neighbors[0]->marker - 2*this->vesselNodes[i]->marker + this->vesselNodes[i]->neighbors[1]->marker) / 2.
						+ flow / 2.;
						//- dt * (outFlow - inFlow) / 2.;
						//+ dt / getVascularVolume( this->vesselNodes[i])
						//		*(vesselNodes[i]->getFlowFromNeighbor(0)*vesselNodes[i]->neighbors[0]->marker
						//		+ vesselNodes[i]->getFlowFromNeighbor(1)*vesselNodes[i]->neighbors[1]->marker) / 2.;
				break;
			}

			case 3: // MACCORMACK / Lax-Wendroff
			{
				dx[i]=0.;
				for( int v=0; v<this->vesselNodes[i]->countNeighbors; v++)
				{
					float courantNumber = this->vesselNodes[i]->getFlowFromNeighbor(v) * dt / this->getVascularVolume( this->vesselNodes[i]);

					// advection
					//dx[i] += courantNumber
					//		* this->vesselNodes[i]->neighbors[v]->marker / 2.;

					// diffusion
					//dx[i] += pow( courantNumber, 2.)
					//		* (this->vesselNodes[i]->neighbors[v]->marker - this->vesselNodes[i]->marker) / 2.;

					// convection = advection + diffusion
					dx[i] += courantNumber*(1.+courantNumber)
							* (this->vesselNodes[i]->neighbors[v]->marker - this->vesselNodes[i]->marker) / 2.;

				}

				break;
			}

			case 4: // PIECEWISE-LINEAR
			{
				//float inFlow = 0.;
				//float outFlow = 0.;
				float flux = 0.;

				// for all flow from/to neighbors
				for( int v=0; v<this->vesselNodes[i]->countNeighbors; v++)
				{
					float flow;// = fabs(this->vesselNodes[i]->branches[v]->flow);
					float sign;
					VesselNode* origin;
					VesselNode* destination;
					if( this->vesselNodes[i]->pressure > this->vesselNodes[i]->neighbors[v]->pressure ){
						// flow to neighbor: f_{i+1/2}
						origin      = this->vesselNodes[i];
						destination = this->vesselNodes[i]->neighbors[v];
						flow = -fabs(this->vesselNodes[i]->branches[v]->flow);
						sign = +1;
					}else{
						// flow from neighbor: f_{i-1/2}
						origin      = this->vesselNodes[i]->neighbors[v];
						destination = this->vesselNodes[i];
						flow = fabs(this->vesselNodes[i]->branches[v]->flow);
						sign = +1;
					}

					// SLOPE
					// Donor cell
					//float slope = 0.;
					// Lax-Wendroff
					//float slope = destination->marker - origin->marker;
					// Beam-Warming
					/*float slope = 0.;
					for( int vv=0; vv<origin->countNeighbors; vv++)
						if( origin->neighbors[vv]!=destination){
							slope += origin->marker - origin->neighbors[vv]->marker;
						}*/
					// Fromm
					float slope = 0.;
					for( int vv=0; vv<origin->countNeighbors; vv++)
						if( origin->neighbors[vv]!=destination){
							slope += 0.5*(destination->marker - origin->neighbors[vv]->marker);
						}

					// SLOPE LIMITER
					/*float predecessorMarker = 0.;
					for( int vv=0; vv<origin->countNeighbors; vv++)
						if( origin->neighbors[vv]!=destination)
							predecessorMarker += origin->neighbors[vv]->marker;
					float r = (origin->marker - predecessorMarker) / (destination->marker - origin->marker);*/

					// minmod slope
					//slope *= MAX(0., MIN(r, 1.));
					//float slope = MIN( MAX( origin->marker - predecessorMarker,0), MAX( destination->marker - origin->marker,0) )
					//			+ MAX( MIN( origin->marker - predecessorMarker,0), MIN( destination->marker - origin->marker,0) );
					// vanLeer slope
					//float slope = (( origin->marker - predecessorMarker)*( destination->marker - origin->marker)>0. ?
					//		2./(1./( origin->marker - predecessorMarker) + 1./( destination->marker - origin->marker)) : 0.);
					// superBee
					//slope *= MAX( 0., MAX( MIN(2.*r,1.), MIN(r,2.) ));
					//float slope = (( origin->marker - predecessorMarker)>0 && ( destination->marker - origin->marker)>0 ? +1 : -1) *
					//		MIN( MIN( fabs(origin->marker - predecessorMarker), fabs( destination->marker - origin->marker) ),
					//			 0.5* MAX( fabs(origin->marker - predecessorMarker), fabs( destination->marker - origin->marker) )	);

					// FLUX
					float courantNumber = flow * dt / this->getVascularVolume( origin);
					flux += flow * (origin->marker + 0.5 * slope * sign * (1. - fabs(courantNumber)));

				}

				dx[i] = flux * dt / this->getVascularVolume( this->vesselNodes[i]);

				break;
			}

			case 5: // LUNA
			{

				float flux = 0.;
				float u_i_minus_1;
				float u_i;
				float u_i_plus_1;

				for( int v=0; v<this->vesselNodes[i]->countNeighbors; v++){
					float cfl;
					if( this->vesselNodes[i]->pressure > this->vesselNodes[i]->neighbors[v]->pressure ){
						// flow to neighbor: f_{i+1/2}
						cfl    = -fabs(this->vesselNodes[i]->branches[v]->flow)  * dt / this->getVascularVolume( this->vesselNodes[i]);
						//sign = +1;

						u_i_plus_1 = this->vesselNodes[i]->neighbors[v]->marker;
						u_i        = this->vesselNodes[i]->marker;
						for( int vv=0; vv<this->vesselNodes[i]->countNeighbors; vv++)
							if( this->vesselNodes[i]->neighbors[vv]!=this->vesselNodes[i]->neighbors[v])
								u_i_minus_1 += this->vesselNodes[i]->neighbors[vv]->marker;
					}else{
						// flow from neighbor: f_{i-1/2}
						cfl    = +fabs(this->vesselNodes[i]->branches[v]->flow) * dt / this->getVascularVolume( this->vesselNodes[i]);
						//sign = +1;
						u_i_plus_1 = this->vesselNodes[i]->marker;
						u_i        = this->vesselNodes[i]->neighbors[v]->marker;
						for( int vv=0; vv<this->vesselNodes[i]->neighbors[v]->countNeighbors; vv++)
							if( this->vesselNodes[i]->neighbors[v]->neighbors[vv]!=this->vesselNodes[i])
								u_i_minus_1 += this->vesselNodes[i]->neighbors[v]->neighbors[vv]->marker;
					}

					float r = (u_i - u_i_minus_1) / (u_i_plus_1 - u_i);
					float slope = this->vesselNodes[i]->neighbors[v]->marker - this->vesselNodes[i]->marker;
					float fluxLimiter = MAX( 0, MIN(r, 1));

					//flux += - 1./2. * cfl * slope
					//	    + (1-(1-(cfl)) * fluxLimiter) * slope;

					//slope =

					// w_i-1
					flux += cfl/2.*(this->vesselNodes[i]->neighbors[v]->marker)
							//- cfl/2.*(1.-fabs(cfl))*fluxLimiter*(this->vesselNodes[i]->neighbors[v]->marker - this->vesselNodes[i]->marker)
							;

					// w_i+1
					//flux += cfl/2.*(this->vesselNodes[i]->neighbors[v]->marker)
					//		- cfl/2.*(1+cfl)*fluxlimiter*(this->vesselNodes[i]->neighbors[v]->marker - this->vesselNodes[i]->marker);

				}

				dx[i] = flux;
				break;
			}
			}
		}
	}

	// ANTI-DIFFUSION
	/*for( int i=0; i<M; i++){
		if( this->vesselNodes[i]->getType() == ROOT && this->vesselNodes[i]->pressure==MAX_PRESSURE)
			this->vesselNodes[i]->marker = border;
		else
			this->vesselNodes[i]->marker += dx[i];
	}
	// calculate raw anti-diffusion fluxes
	float *fad = (float*) malloc( sizeof(float) * this->countVesselSegments);
	for( int i=0; i<countVesselSegments; i++){

		float courantNumber = this->vesselSegments[i]->flow;// * dt / this->getVascularVolume( this->vesselNodes[i]);
		float diffusionCoefficient = fabs(courantNumber) / 2.;
		float antiDiffCoefficient  = 4.*diffusionCoefficient ;

		//float diffusionCoefficient = fabs(this->vesselSegments[i]->flow) / 2.;
		//float antiDiffCoefficient  = diffusionCoefficient - fabs(this->vesselSegments[i]->flow) / 2.;

		fad[i] = antiDiffCoefficient
				* (this->vesselSegments[i]->vesselNodes[1]->marker - this->vesselSegments[i]->vesselNodes[0]->marker);
	}
	// limite/correct fluxes
	for( int i=0; i<countVesselSegments; i++){

		float courantNumber = this->vesselSegments[i]->flow;// * dt / this->getVascularVolume( this->vesselNodes[i]);
		float diffusionCoefficient = fabs(courantNumber) / 2.;
		float antiDiffCoefficient  = 5.*diffusionCoefficient;


		float sign = (fad[i]<0. ? -1 : 1);
		float min = fabs( fad[i]);

		// p_i+2 - p_i+1
		float temp=0.;
		for( int ii=0; ii<this->vesselSegments[i]->vesselNodes[1]->countNeighbors; ii++)
			if(this->vesselSegments[i]->vesselNodes[1]->branches[ii] != this->vesselSegments[i]){
				temp += antiDiffCoefficient*sign * (
						// p_i+2
						this->vesselSegments[i]->vesselNodes[1]->neighbors[ii]->marker
						// p_i+1
						- this->vesselSegments[i]->vesselNodes[1]->marker);
			}
		fprintf( stderr, "[%i] fad_i+1 = %e\n", i, temp);
		min = MIN( min, temp);

		// p_i - p_i-1
		temp=0.;
		for( int ii=0; ii<this->vesselSegments[i]->vesselNodes[0]->countNeighbors; ii++)
			if(this->vesselSegments[i]->vesselNodes[0]->branches[ii] != this->vesselSegments[i]){
				temp += antiDiffCoefficient*sign * (
						// p_i
						this->vesselSegments[i]->vesselNodes[0]->marker
						// p_i-1
						- this->vesselSegments[i]->vesselNodes[0]->neighbors[ii]->marker);
			}
		fprintf( stderr, "[%i] fad_i-1 = %e\n", i, temp);
		min = MIN( min, temp);

		// correct fad
		fprintf( stderr, "[%i] fad = %e ---> corrected = %e\n", i, fad[i], sign * MAX(0, min));
		fad[i] = sign * MAX(0, min);
	}
	// perform antidiffusive correction
	for( int i=0; i<M; i++){
		dx[i] = 0.;
	}
	for( int i=0; i<countVesselSegments; i++){
		dx[this->vesselSegments[i]->vesselNodes[0]->index] -= fad[i] * dt / this->getVascularVolume( this->vesselSegments[i]->vesselNodes[0]);
		dx[this->vesselSegments[i]->vesselNodes[1]->index] += fad[i] * dt / this->getVascularVolume( this->vesselSegments[i]->vesselNodes[1]);
	}
*/
	for( int i=0; i<M; i++){
		if( this->vesselNodes[i]->getType() == ROOT && this->vesselNodes[i]->pressure==MAX_PRESSURE)
			this->vesselNodes[i]->marker = border;
		else
			this->vesselNodes[i]->marker += dx[i];
	}
}


void VesselGraph::updateMarkerVesselsExplicitAntiDiffusion( float dt, float border, float *&dx)
{
	int M = this->countVesselNodes;
	if(dx==0)
	dx = (float*) malloc( sizeof(float) * (M));


	// INIT MARKER CONC.
	for( int i=0; i<M; i++){
		dx[i] = this->vesselNodes[i]->marker;
	}


	// Construct Matrix for Vascular Marker Concentration
	for( int i=0; i<M; i++){
		if( this->vesselNodes[i]->getType() == ROOT && this->vesselNodes[i]->pressure==MAX_PRESSURE){

			// BOUNDARY: ROOT
			//fprintf( stderr, "Found Root with pressure=%f\n", this->vesselNodes[i]->pressure);
			dx[i] = (border - this->vesselNodes[i]->marker) / dt;

		}else{

			// c_P^n+1
			dx[i] = 0.;
			float inFlow = 0.;
			float outFlow = 0.;
			float antiDiffusion = 0.;
			for( int v=0; v<this->vesselNodes[i]->countNeighbors; v++)
			{
				// out flow
				if( this->vesselNodes[i]->pressure >= this->vesselNodes[i]->neighbors[v]->pressure)
				//	x[i] -= this->vesselNodes[i]->marker * fabs(this->vesselNodes[i]->branches[v]->flow);
					antiDiffusion += PI * pow(this->vesselNodes[i]->branches[v]->radius,2) / 60.
									* (this->vesselNodes[i]->marker - this->vesselNodes[i]->neighbors[v]->marker);

				// in flow
				if( this->vesselNodes[i]->pressure < this->vesselNodes[i]->neighbors[v]->pressure){
					//dx[i] += this->vesselNodes[i]->neighbors[v]->marker * fabs(this->vesselNodes[i]->branches[v]->flow);

					inFlow  += fabs(this->vesselNodes[i]->branches[v]->flow) * this->vesselNodes[i]->neighbors[v]->marker;
					outFlow += fabs(this->vesselNodes[i]->branches[v]->flow) * this->vesselNodes[i]->marker;
					antiDiffusion += PI * pow(this->vesselNodes[i]->branches[v]->radius,2) / 60.
									* (this->vesselNodes[i]->marker - this->vesselNodes[i]->neighbors[v]->marker);

				}
			}
			dx[i] = (inFlow - outFlow + 100.*antiDiffusion) / this->getVascularVolume( this->vesselNodes[i]);
		}
	}

	for( int i=0; i<M; i++){
		if( this->vesselNodes[i]->getType() == ROOT && this->vesselNodes[i]->pressure==MAX_PRESSURE)
			this->vesselNodes[i]->marker = border;
		else
			this->vesselNodes[i]->marker += dt * dx[i];
	}
}


void VesselGraph::updateMarkerVesselsExplicit( float dt, float border, float *&dx, float *&x)
{
	int countVesselSegmentSubNodes = 100;
	int N = this->countVesselSegments * countVesselSegmentSubNodes;
	int M = this->countVesselNodes;
	if(dx==0){
		dx = (float*) malloc( sizeof(float) * (M + N));
		for( int i=0; i<M+N; i++)
			dx[i]=0;
	}
	if(x==0){
		x = (float*) malloc( sizeof(float) * (M + N));
		for( int i=0; i<M+N; i++)
			x[i]=0;
	}


	// INIT MARKER CONC.
	for( int i=0; i<M; i++){
		x[i] = this->vesselNodes[i]->marker;
	}


	// Construct Matrix for Vascular Marker Concentration: vessel nodes
	for( int i=0; i<M; i++)
	{
		if( this->vesselNodes[i]->getType() == ROOT && this->vesselNodes[i]->pressure==MAX_PRESSURE){

			// BOUNDARY: ROOT
			//fprintf( stderr, "Found Root with pressure=%f\n", this->vesselNodes[i]->pressure);
			dx[i] = (border - this->vesselNodes[i]->marker) / dt;

		}else{

			// c_P^n+1
			dx[i] = 0.;
			float inFlow = 0.;
			float outFlow = 0.;
			for( int v=0; v<this->vesselNodes[i]->countNeighbors; v++)
			{
				// 3-POINT
				if( false){
					// out flow
					//if( this->vesselNodes[i]->pressure >= this->vesselNodes[i]->neighbors[v]->pressure)
					//	x[i] -= this->vesselNodes[i]->marker * fabs(this->vesselNodes[i]->branches[v]->flow);
				}

				// EXPLICIT UPWIND
				if( true){
					// in flow
					if( this->vesselNodes[i]->pressure < this->vesselNodes[i]->neighbors[v]->pressure){

						float neighborConc = x[M + (1+this->vesselNodes[i]->branches[v]->index)*countVesselSegmentSubNodes-1];

						inFlow  += fabs(this->vesselNodes[i]->branches[v]->flow) * neighborConc;
						outFlow += fabs(this->vesselNodes[i]->branches[v]->flow) * this->vesselNodes[i]->marker;
					}
				}
			}
			dx[i] = (inFlow - outFlow) / (this->getVascularVolume( this->vesselNodes[i]) / (countVesselSegmentSubNodes+1.));
		}
	}

	// Construct Matrix for Vascular Marker Concentration: vessel segment sub nodes
	for( int i=0; i<this->countVesselSegments; i++){
		//fprintf( stderr, "%i == %i?\n", i, this->vesselSegments[i]->index);

		float flow = fabs(this->vesselSegments[i]->flow) / (this->getVascularVolume( this->vesselSegments[i]) / (countVesselSegmentSubNodes+1.));

		int index = M + i*countVesselSegmentSubNodes;

		if(this->vesselSegments[i]->vesselNodes[0]->pressure > this->vesselSegments[i]->vesselNodes[1]->pressure){
			// flow direction [0] -> [1]
			dx[index] = flow*( this->vesselSegments[i]->vesselNodes[0]->marker - x[index] );
		}else{
			// flow direction [1] -> [0]
			dx[index] = flow*( this->vesselSegments[i]->vesselNodes[1]->marker - x[index] );
		}
		//if(this->vesselSegments[i]->vesselNodes[0]->pressure == MAX_PRESSURE || this->vesselSegments[i]->vesselNodes[1]->pressure == MAX_PRESSURE)
		//fprintf( stderr, "dx = %e, x=%e\n", dx[index], x[index]);
		for( int ii=1; ii<countVesselSegmentSubNodes; ii++)
		{
			index++;
			dx[index] = flow*( x[index-1] - x[index] );
			//if(this->vesselSegments[i]->vesselNodes[0]->pressure == MAX_PRESSURE || this->vesselSegments[i]->vesselNodes[1]->pressure == MAX_PRESSURE)
			//	fprintf( stderr, "%i ---> dx = %e, x=%e\n", ii, dx[index], x[index]);
		}
	}

	for( int i=0; i<M; i++){
		if( this->vesselNodes[i]->getType() == ROOT && this->vesselNodes[i]->pressure==MAX_PRESSURE)
			this->vesselNodes[i]->marker = border;
		else
			this->vesselNodes[i]->marker += dt * dx[i];
	}

	for( int i=M; i<M+N; i++){
		x[i] += dt * dx[i];
	}
}


float VesselGraph::getExtraVascularVolume( VesselNode *vn)
{
	// volume: tissue
	float volume = LATTICE_CONSTANT*LATTICE_CONSTANT*LATTICE_CONSTANT;
	if( vn!=0)
	for( int v=0; v<vn->countNeighbors; v++)
		volume -=
			0.5
			* LATTICE_CONSTANT
			* PI * vn->branches[v]->radius * vn->branches[v]->radius;

	if( volume<0){
		fprintf( stderr, "ERROR: extra vascular volume is negative (%lf)!!\n", volume);
		exit(0);
	}

	return volume;
}

float VesselGraph::getExtraVascularVolume( int &x, int &y, int &z)
{
	// volume: tissue
	VesselNode *vn = this->octree->at( x,y,z);
	float volume = LATTICE_CONSTANT*LATTICE_CONSTANT*LATTICE_CONSTANT;
	if( vn!=0)
	for( int v=0; v<vn->countNeighbors; v++)
		volume -=
			0.5
			* LATTICE_CONSTANT
			* PI * vn->branches[v]->radius * vn->branches[v]->radius;

	if( volume<0){
		fprintf( stderr, "ERROR: extra vascular volume is negative (%lf)!!\n", volume);
		exit(0);
	}

	return volume;
}

float VesselGraph::getVascularSurface( VesselSegment *vs)
{
	// volume: vessel segment
	if( vs!=0)
		return LATTICE_CONSTANT
			* 2. * PI * vs->radius;

	return 0;
}

float VesselGraph::getVascularSurface( VesselNode *vn)
{
	// volume: vessel segment
	float surface = 0.;

	if( vn!=0)
	for( int v=0; v<vn->countNeighbors; v++)
		surface += 0.5 * getVascularSurface(vn->branches[v]);

	return surface;
}

float VesselGraph::getVascularPermeabilitySurfaceProduct( VesselNode *vn)
{
	// volume: vessel segment
	float permeabilitySurfaceProduct = 0.;

	if( vn!=0)
	for( int v=0; v<vn->countNeighbors; v++)
		permeabilitySurfaceProduct += 0.5 * getVascularSurface(vn->branches[v]) * vn->branches[v]->permeability;

	return permeabilitySurfaceProduct;
}

float VesselGraph::getVascularVolume( VesselSegment *vs)
{
	// volume: vessel segment
	if( vs!=0)
		return LATTICE_CONSTANT
			* PI * vs->radius * vs->radius;

	return 0;
}

float VesselGraph::getVascularVolume( VesselNode *vn)
{
	// volume: tissue
	float volume = 0.;
	if( vn!=0)
	for( int v=0; v<vn->countNeighbors; v++)
		volume +=
			0.5
			* LATTICE_CONSTANT
			* PI * vn->branches[v]->radius * vn->branches[v]->radius;

	if( volume>LATTICE_CONSTANT*LATTICE_CONSTANT*LATTICE_CONSTANT){
		fprintf( stderr, "ERROR: extra vascular volume is larger than voxel size (%lf < %lf)!!\n", volume, LATTICE_CONSTANT*LATTICE_CONSTANT*LATTICE_CONSTANT);
		exit(0);
	}

	return volume;
}

float VesselGraph::getVascularVolume( int &x, int &y, int &z)
{
	// volume: tissue
	VesselNode *vn = this->octree->at( x,y,z);
	float volume = 0.;
	if( vn!=0)
	for( int v=0; v<vn->countNeighbors; v++)
		volume +=
			0.5
			* LATTICE_CONSTANT
			* PI * vn->branches[v]->radius * vn->branches[v]->radius;

	if( volume>LATTICE_CONSTANT*LATTICE_CONSTANT*LATTICE_CONSTANT){
		fprintf( stderr, "ERROR: extra vascular volume is larger than voxel size (%lf < %lf)!!\n", volume, LATTICE_CONSTANT*LATTICE_CONSTANT*LATTICE_CONSTANT);
		exit(0);
	}

	return volume;
}

float VesselNode::getFlowFromNeighbor( int i)
{
	assert(i<this->countNeighbors);

	return (this->neighbors[i]->pressure >= this->pressure ?

			// IN FLOW defined POSITIVE
			+ fabs(this->branches[i]->flow) :

			// OUT FLOW defined NEGATIVE
			- fabs(this->branches[i]->flow));
}

float VesselNode::getFlowToNeighbor( int i)
{
	assert(i<this->countNeighbors);

	return (this->pressure >= this->neighbors[i]->pressure ?

			// OUT FLOW defined POSITIVE
			+ fabs(this->branches[i]->flow) :

			// IN FLOW defined NEGATIVE
			- fabs(this->branches[i]->flow));
}

void VesselGraph::updateMarkerVesselsAndInterstitialSpaceNEW3( float dt, float border, float ***markerIntSpaceIn, float ***markerIntSpaceOut)
{
	float *b=0;
	float *x=0;
	int M = this->countVesselNodes;
	int N = DOMAIN_SIZE_X*DOMAIN_SIZE_Y*DOMAIN_SIZE_Z;

	SparseMatrix<float> *sA = new SparseMatrix<float>(M+N,M+N);
	b = (float*) malloc( sizeof(float) * (M+N));
	x = (float*) malloc( sizeof(float) * (M+N));

	Solver<float> *S = new Solver<float>( M+N, Solver<float>::BiCGSTAB );


	// INIT MARKER CONC.
	for( int i=0; i<M; i++){
		x[i] = this->vesselNodes[i]->marker;
	}
	for( int i=M; i<M+N; i++){
		x[i] = 0;
	}

	// Construct Matrix for Vascular Marker Concentration
	//float volume = 0.5;
//#pragma omp parallel for
	for( int i=0; i<M; i++){
		if( this->vesselNodes[i]->countNeighbors == 0)
			fprintf( stderr, "ERROR: Node without neighbors!\n");
		if( this->vesselNodes[i]->getType() == ROOT){
			// BOUNDARY: ROOT
			sA->set(i,i, 1);
			if( this->vesselNodes[i]->pressure==MAX_PRESSURE)
				//b[i] = ARTERIAL_MARKER_CONC;
				b[i] = border;
			else
				b[i] = 0;
		}else{
			//float r=volume/dt + EXCHANGE_RATE_VESSELS_INTERSTITIAL;
			//float sumFlow=0.;

			// volume
			float volume = 0.;
			for( int v=0; v<this->vesselNodes[i]->countNeighbors; v++)
				volume += 0.5*LATTICE_CONSTANT
						* PI * this->vesselNodes[i]->branches[v]->radius*this->vesselNodes[i]->branches[v]->radius;


			// c_P^n+1
			sA->set(i,i, 1);

			// r
			float r = 0.;
			for( int v=0; v<this->vesselNodes[i]->countNeighbors; v++)
			{
				// out flow
				if( this->vesselNodes[i]->pressure >= this->vesselNodes[i]->neighbors[v]->pressure)
					r += fabs(this->vesselNodes[i]->branches[v]->flow);
			}
			r = 1. + dt/volume*r + dt/volume*EXCHANGE_RATE_VESSELS_INTERSTITIAL;

			// c_P,i^n+1 (for all neighbors i)
			for( int v=0; v<this->vesselNodes[i]->countNeighbors; v++)
			{
				float flow;
				// in flow
				if( this->vesselNodes[i]->pressure < this->vesselNodes[i]->neighbors[v]->pressure){
					flow = -fabs(this->vesselNodes[i]->branches[v]->flow);
					sA->set(i,this->vesselNodes[i]->neighbors[v]->index, flow*dt/r/volume);
					if( isnan(flow*dt/r/volume)){
						fprintf( stderr, "flow=%lf dt=%lf r=%lf volume=%lf\n",flow,dt,r,volume);
						exit(0);
					}
				}
			}

			// c_I^n+1
			int index = this->vesselNodes[i]->position[0]
			          + this->vesselNodes[i]->position[1]*DOMAIN_SIZE_X
			          + this->vesselNodes[i]->position[2]*DOMAIN_SIZE_X*DOMAIN_SIZE_Y;
			sA->set(i,M+index, -EXCHANGE_RATE_VESSELS_INTERSTITIAL*dt/r/volume);
			if( isnan(EXCHANGE_RATE_VESSELS_INTERSTITIAL*dt/r/volume)){
				fprintf( stderr, "EXCHANGE_RATE_VESSELS_INTERSTITIAL=%lf dt=%lf r=%lf volume=%lf\n",EXCHANGE_RATE_VESSELS_INTERSTITIAL,dt,r,volume);
				exit(0);
			}

			// b
			b[i] = 1./r * this->vesselNodes[i]->marker;
			if( isnan(b[i])){
				fprintf( stderr, "r=%lf this->vesselNodes[i]->marker=%lf\n",r,this->vesselNodes[i]->marker);
				exit(0);
			}

		}
	}

	// Construct Matrix for Interstitial Marker Concentration
//#pragma omp parallel for
	for( int z=0; z<DOMAIN_SIZE_Z; z++)
		for( int y=0; y<DOMAIN_SIZE_Y; y++)
			for( int x=0; x<DOMAIN_SIZE_X; x++)
			{
				int i = M
					  + x
				      + y*DOMAIN_SIZE_X
				      + z*DOMAIN_SIZE_X*DOMAIN_SIZE_Y;

				// volume: tissue
				VesselNode *vn = this->octree->at( x,y,z);
				float volume = LATTICE_CONSTANT*LATTICE_CONSTANT*LATTICE_CONSTANT;
				if( vn!=0)
				for( int v=0; v<vn->countNeighbors; v++)
					volume -= 0.5*LATTICE_CONSTANT
						* PI * vn->branches[v]->radius*vn->branches[v]->radius;

				if(volume < 0.){
					fprintf( stderr, "ERROR: negative volume: %lf!!\n", volume);
					fprintf( stderr, "ERROR: max volume: %lf = %lf^3\n", (float)LATTICE_CONSTANT*LATTICE_CONSTANT*LATTICE_CONSTANT, (float)LATTICE_CONSTANT);
					for( int v=0; v<vn->countNeighbors; v++)
						fprintf( stderr, "%i neighbor: radius=%lf, volume=%lf\n", v, vn->branches[v]->radius, 0.5*LATTICE_CONSTANT	* PI * vn->branches[v]->radius*vn->branches[v]->radius);
					exit(0);
				}

				// c_I^n+1
				sA->set(i,i, 1);

				// r
				//float r=(1.-volume)/dt + EXCHANGE_RATE_VESSELS_INTERSTITIAL;
				float r = 0.;
				for( int nx=(x>0?x-1:x+1); nx<=x+1 && nx<DOMAIN_SIZE_X; nx+=2){
					r+= 1. + this->getExtraVascularVolume( nx, y, z) / volume;
				}
				for( int ny=(y>0?y-1:y+1); ny<=y+1 && ny<DOMAIN_SIZE_Y; ny+=2){
					r+= 1. + this->getExtraVascularVolume( x, ny, z) / volume;
				}
				for( int nz=(z>0?z-1:z+1); nz<=z+1 && nz<DOMAIN_SIZE_Z; nz+=2){
					r+= 1. + this->getExtraVascularVolume( x, y, nz) / volume;
				}
				r = 1.
				  + dt * MARKER_DIFFUSION_COEFFICIENT /(LATTICE_CONSTANT*LATTICE_CONSTANT) * r/2.
				  + dt * EXCHANGE_RATE_VESSELS_INTERSTITIAL/volume;

				// c_I,i^n+1 (for all neighbors i)
				for( int nx=(x>0?x-1:x+1); nx<=x+1 && nx<DOMAIN_SIZE_X; nx+=2)
				{
					int ii = M
						   + nx
					       + y*DOMAIN_SIZE_X
					       + z*DOMAIN_SIZE_X*DOMAIN_SIZE_Y;
					sA->set(i,ii, -MARKER_DIFFUSION_COEFFICIENT/(LATTICE_CONSTANT*LATTICE_CONSTANT) * dt/r * (1. + this->getExtraVascularVolume( nx, y, z) / volume)/2.);

					if( isnan(MARKER_DIFFUSION_COEFFICIENT/(LATTICE_CONSTANT*LATTICE_CONSTANT) * dt/r)){
						fprintf( stderr, "r=%lf\n",r);
						exit(0);
					}
				}
				for( int ny=(y>0?y-1:y+1); ny<=y+1 && ny<DOMAIN_SIZE_Y; ny+=2)
				{
					int ii = M
						   + x
					       + ny*DOMAIN_SIZE_X
					       + z*DOMAIN_SIZE_X*DOMAIN_SIZE_Y;
					sA->set(i,ii, -MARKER_DIFFUSION_COEFFICIENT/(LATTICE_CONSTANT*LATTICE_CONSTANT) * dt/r * (1. + this->getExtraVascularVolume( x, ny, z) / volume)/2.);
					if( isnan(MARKER_DIFFUSION_COEFFICIENT/(LATTICE_CONSTANT*LATTICE_CONSTANT) * dt/r)){
						fprintf( stderr, "r=%lf\n",r);
						exit(0);
					}
				}
				for( int nz=(z>0?z-1:z+1); nz<=z+1 && nz<DOMAIN_SIZE_Z; nz+=2)
				{
					int ii = M
						   + x
					       + y*DOMAIN_SIZE_X
					       + nz*DOMAIN_SIZE_X*DOMAIN_SIZE_Y;
					sA->set(i,ii, -MARKER_DIFFUSION_COEFFICIENT/(LATTICE_CONSTANT*LATTICE_CONSTANT) * dt/r * (1. + this->getExtraVascularVolume( x, y, nz) / volume)/2.);
					if( isnan(MARKER_DIFFUSION_COEFFICIENT/(LATTICE_CONSTANT*LATTICE_CONSTANT) * dt/r)){
						fprintf( stderr, "r=%lf\n",r);
						exit(0);
					}
				}

				// c_P^n+1
				if( vn!=0){
					sA->set(i,vn->index, -EXCHANGE_RATE_VESSELS_INTERSTITIAL/volume * dt/r);
					if( isnan(-EXCHANGE_RATE_VESSELS_INTERSTITIAL/volume * dt/r)){
						fprintf( stderr, "r=%lf volume=%lf\n",r,volume);
						exit(0);
					}
				}

				// b
				b[i] = 1./r * markerIntSpaceIn[x][y][z];
				if( isnan(b[i])){
					fprintf( stderr, "r=%lf markerIntSpaceIn[x][y][z]=%lf\n",r,markerIntSpaceIn[x][y][z]);
					exit(0);
				}
			}


	//SolveBiCGSTAB( sA, b, x, 20000, 1e-10);

	//ConjugateGradientSparse( sA, b, x, 20000, 1e-20);
	//fprintf( stderr, "Precondition System\n");
	S->PreconditionJacobi( sA, b);
	//fprintf( stderr, "Solve System\n");
	S->Solve( sA, b, x);
	delete S;

	for( int i=0; i<M; i++)
		this->vesselNodes[i]->marker = x[i];
	for( int iz=0; iz<DOMAIN_SIZE_Z; iz++)
		for( int iy=0; iy<DOMAIN_SIZE_Y; iy++)
			for( int ix=0; ix<DOMAIN_SIZE_X; ix++)
			{
				int i = M
					  + ix
				      + iy*DOMAIN_SIZE_X
				      + iz*DOMAIN_SIZE_X*DOMAIN_SIZE_Y;
				markerIntSpaceOut[ix][iy][iz] = x[i];
			}


	delete sA;
	free(b);
	free(x);
}


void VesselGraph::updateMarkerVesselsAndInterstitialSpaceNEW4( float dt, float border, float ***markerIntSpaceIn, float ***markerIntSpaceOut)
{
	float *b=0;
	float *x=0;
	int M = this->countVesselNodes;
	int N = DOMAIN_SIZE_X*DOMAIN_SIZE_Y*DOMAIN_SIZE_Z;

	SparseMatrix<float> *sA = new SparseMatrix<float>(M+N,M+N);
	b = (float*) malloc( sizeof(float) * (M+N));
	x = (float*) malloc( sizeof(float) * (M+N));

	Solver<float> *S = new Solver<float>( M+N, Solver<float>::BiCGSTAB );


	// INIT MARKER CONC.
	for( int i=0; i<M; i++){
		x[i] = this->vesselNodes[i]->marker;
	}
	for( int i=M; i<M+N; i++){
		x[i] = 0;
	}

	// Construct Matrix for Vascular Marker Concentration
	//float volume = 0.5;
//#pragma omp parallel for
	for( int i=0; i<M; i++){
		if( this->vesselNodes[i]->countNeighbors == 0)
			fprintf( stderr, "ERROR: Node without neighbors!\n");
		if( this->vesselNodes[i]->getType() == ROOT){
			// BOUNDARY: ROOT
			sA->set(i,i, 1);
			if( this->vesselNodes[i]->pressure==MAX_PRESSURE)
				//b[i] = ARTERIAL_MARKER_CONC;
				b[i] = border;
			else
				b[i] = 0;
		}else{
			//float r=volume/dt + EXCHANGE_RATE_VESSELS_INTERSTITIAL;
			//float sumFlow=0.;

			// volume
			float volume = 0.;
			for( int v=0; v<this->vesselNodes[i]->countNeighbors; v++)
				volume += 0.5*LATTICE_CONSTANT
						* PI * this->vesselNodes[i]->branches[v]->radius*this->vesselNodes[i]->branches[v]->radius;


			// c_P^n+1
			sA->set(i,i, 1);

			// r
			float r = 0.;
			for( int v=0; v<this->vesselNodes[i]->countNeighbors; v++)
			{
				// out flow
				if( this->vesselNodes[i]->pressure >= this->vesselNodes[i]->neighbors[v]->pressure)
					r += fabs(this->vesselNodes[i]->branches[v]->flow);
			}
			r = 1. + dt/volume*r + dt/volume * this->getVascularPermeabilitySurfaceProduct( vesselNodes[i]);

			// c_P,i^n+1 (for all neighbors i)
			for( int v=0; v<this->vesselNodes[i]->countNeighbors; v++)
			{
				float flow;
				// in flow
				if( this->vesselNodes[i]->pressure < this->vesselNodes[i]->neighbors[v]->pressure){
					flow = -fabs(this->vesselNodes[i]->branches[v]->flow);
					sA->set(i,this->vesselNodes[i]->neighbors[v]->index, flow*dt/r/volume);
					if( isnan(flow*dt/r/volume)){
						fprintf( stderr, "flow=%lf dt=%lf r=%lf volume=%lf\n",flow,dt,r,volume);
						exit(0);
					}
				}
			}

			// c_I^n+1
			int index = this->vesselNodes[i]->position[0]
			          + this->vesselNodes[i]->position[1]*DOMAIN_SIZE_X
			          + this->vesselNodes[i]->position[2]*DOMAIN_SIZE_X*DOMAIN_SIZE_Y;
			sA->set(i,M+index, -this->getVascularPermeabilitySurfaceProduct( vesselNodes[i])*dt/r/volume);
			if( isnan(EXCHANGE_RATE_VESSELS_INTERSTITIAL*dt/r/volume)){
				fprintf( stderr, "EXCHANGE_RATE_VESSELS_INTERSTITIAL=%lf dt=%lf r=%lf volume=%lf\n",EXCHANGE_RATE_VESSELS_INTERSTITIAL,dt,r,volume);
				exit(0);
			}

			// b
			b[i] = 1./r * this->vesselNodes[i]->marker;
			if( isnan(b[i])){
				fprintf( stderr, "r=%lf this->vesselNodes[i]->marker=%lf\n",r,this->vesselNodes[i]->marker);
				exit(0);
			}

		}
	}

	// Construct Matrix for Interstitial Marker Concentration
//#pragma omp parallel for
	for( int z=0; z<DOMAIN_SIZE_Z; z++)
		for( int y=0; y<DOMAIN_SIZE_Y; y++)
			for( int x=0; x<DOMAIN_SIZE_X; x++)
			{
				int i = M
					  + x
				      + y*DOMAIN_SIZE_X
				      + z*DOMAIN_SIZE_X*DOMAIN_SIZE_Y;

				VesselNode *vn = this->octree->at( x,y,z);

				// volume: tissue
				float volume = this->getExtraVascularVolume(vn);

				// marker exchange
				float exchange_rate = this->getVascularPermeabilitySurfaceProduct( vn);

				// c_I^n+1
				sA->set(i,i, 1);

				// r
				//float r=(1.-volume)/dt + EXCHANGE_RATE_VESSELS_INTERSTITIAL;
				float r = 0.;
				for( int nx=(x>0?x-1:x+1); nx<=x+1 && nx<DOMAIN_SIZE_X; nx+=2){
					r+= 1. + this->getExtraVascularVolume( nx, y, z) / volume;
				}
				for( int ny=(y>0?y-1:y+1); ny<=y+1 && ny<DOMAIN_SIZE_Y; ny+=2){
					r+= 1. + this->getExtraVascularVolume( x, ny, z) / volume;
				}
				for( int nz=(z>0?z-1:z+1); nz<=z+1 && nz<DOMAIN_SIZE_Z; nz+=2){
					r+= 1. + this->getExtraVascularVolume( x, y, nz) / volume;
				}
				r = 1.
				  + dt * MARKER_DIFFUSION_COEFFICIENT /(LATTICE_CONSTANT*LATTICE_CONSTANT) * r/2.
				  + dt * exchange_rate/volume;

				// c_I,i^n+1 (for all neighbors i)
				for( int nx=(x>0?x-1:x+1); nx<=x+1 && nx<DOMAIN_SIZE_X; nx+=2)
				{
					int ii = M
						   + nx
					       + y*DOMAIN_SIZE_X
					       + z*DOMAIN_SIZE_X*DOMAIN_SIZE_Y;
					sA->set(i,ii, -MARKER_DIFFUSION_COEFFICIENT/(LATTICE_CONSTANT*LATTICE_CONSTANT) * dt/r * (1. + this->getExtraVascularVolume( nx, y, z) / volume)/2.);

					if( isnan(MARKER_DIFFUSION_COEFFICIENT/(LATTICE_CONSTANT*LATTICE_CONSTANT) * dt/r)){
						fprintf( stderr, "r=%lf\n",r);
						exit(0);
					}
				}
				for( int ny=(y>0?y-1:y+1); ny<=y+1 && ny<DOMAIN_SIZE_Y; ny+=2)
				{
					int ii = M
						   + x
					       + ny*DOMAIN_SIZE_X
					       + z*DOMAIN_SIZE_X*DOMAIN_SIZE_Y;
					sA->set(i,ii, -MARKER_DIFFUSION_COEFFICIENT/(LATTICE_CONSTANT*LATTICE_CONSTANT) * dt/r * (1. + this->getExtraVascularVolume( x, ny, z) / volume)/2.);
					if( isnan(MARKER_DIFFUSION_COEFFICIENT/(LATTICE_CONSTANT*LATTICE_CONSTANT) * dt/r)){
						fprintf( stderr, "r=%lf\n",r);
						exit(0);
					}
				}
				for( int nz=(z>0?z-1:z+1); nz<=z+1 && nz<DOMAIN_SIZE_Z; nz+=2)
				{
					int ii = M
						   + x
					       + y*DOMAIN_SIZE_X
					       + nz*DOMAIN_SIZE_X*DOMAIN_SIZE_Y;
					sA->set(i,ii, -MARKER_DIFFUSION_COEFFICIENT/(LATTICE_CONSTANT*LATTICE_CONSTANT) * dt/r * (1. + this->getExtraVascularVolume( x, y, nz) / volume)/2.);
					if( isnan(MARKER_DIFFUSION_COEFFICIENT/(LATTICE_CONSTANT*LATTICE_CONSTANT) * dt/r)){
						fprintf( stderr, "r=%lf\n",r);
						exit(0);
					}
				}

				// c_P^n+1
				if( vn!=0){
					sA->set(i,vn->index, -exchange_rate/volume * dt/r);
					if( isnan(-exchange_rate/volume * dt/r)){
						fprintf( stderr, "r=%lf volume=%lf\n",r,volume);
						exit(0);
					}
				}

				// b
				b[i] = 1./r * markerIntSpaceIn[x][y][z];
				if( isnan(b[i])){
					fprintf( stderr, "r=%lf markerIntSpaceIn[x][y][z]=%lf\n",r,markerIntSpaceIn[x][y][z]);
					exit(0);
				}
			}


	//SolveBiCGSTAB( sA, b, x, 20000, 1e-10);

	//ConjugateGradientSparse( sA, b, x, 20000, 1e-20);
	//fprintf( stderr, "Precondition System\n");
	S->PreconditionJacobi( sA, b);
	//fprintf( stderr, "Solve System\n");
	S->Solve( sA, b, x);
	delete S;

	for( int i=0; i<M; i++)
		this->vesselNodes[i]->marker = x[i];
	for( int iz=0; iz<DOMAIN_SIZE_Z; iz++)
		for( int iy=0; iy<DOMAIN_SIZE_Y; iy++)
			for( int ix=0; ix<DOMAIN_SIZE_X; ix++)
			{
				int i = M
					  + ix
				      + iy*DOMAIN_SIZE_X
				      + iz*DOMAIN_SIZE_X*DOMAIN_SIZE_Y;
				markerIntSpaceOut[ix][iy][iz] = x[i];
			}


	delete sA;
	free(b);
	free(x);
}


void VesselGraph::updateMarkerVesselsAndInterstitialSpaceBrix( float dt, float **markerVesselsIn, float **markerVesselsOut, float ***markerIntSpaceIn, float ***markerIntSpaceOut)
{
	float *b=0;
	float *x=0;
	int M = this->countVesselNodes;
	int N = DOMAIN_SIZE_X*DOMAIN_SIZE_Y*DOMAIN_SIZE_Z;
	//if(sA==0)
	SparseMatrix<float> *sA = new SparseMatrix<float>(M+N,M+N);
	//if(b==0)
	b = (float*) malloc( sizeof(float) * (M+N));
	//if(x==0)
	x = (float*) malloc( sizeof(float) * (M+N));

	/*v0 = (float*) malloc( sizeof(float) * (M+N));
	v1 = (float*) malloc( sizeof(float) * (M+N));
	v2 = (float*) malloc( sizeof(float) * (M+N));
	v3 = (float*) malloc( sizeof(float) * (M+N));
	v4 = (float*) malloc( sizeof(float) * (M+N));
	v5 = (float*) malloc( sizeof(float) * (M+N));
	v6 = (float*) malloc( sizeof(float) * (M+N));
*/

	// INIT PRESSURE VALUES
	for( int i=0; i<M; i++){
		x[i] = this->vesselNodes[i]->marker;
	}

	// Construct Matrix for Vascular Marker Concentration
	//float volume = 0.5;
//#pragma omp parallel for
	for( int i=0; i<M; i++){
		if( this->vesselNodes[i]->countNeighbors == 0)
			fprintf( stderr, "ERROR: Node without neighbors!\n");
		if( this->vesselNodes[i]->getType() == ROOT){
			// BOUNDARY: ROOT
			sA->set(i,i, 1);
			if( this->vesselNodes[i]->pressure==MAX_PRESSURE)
				b[i] = ARTERIAL_MARKER_CONC;
			else
				b[i] = 0;
		}else{
			//float r=volume/dt + EXCHANGE_RATE_VESSELS_INTERSTITIAL;
			//float sumFlow=0.;

			// volume
			float volume = 0.;
			for( int v=0; v<this->vesselNodes[i]->countNeighbors; v++)
				volume += 0.5*LATTICE_CONSTANT
						* PI * this->vesselNodes[i]->branches[v]->radius*this->vesselNodes[i]->branches[v]->radius;


			// c_P^n+1
			sA->set(i,i, 1);

			// r
			float flowSum = 0.;
			for( int v=0; v<this->vesselNodes[i]->countNeighbors; v++)
			{
				// out flow
				if( this->vesselNodes[i]->pressure >= this->vesselNodes[i]->neighbors[v]->pressure){
					flowSum += fabs(this->vesselNodes[i]->branches[v]->flow);
				}
			}
			float r = 0.;
			r = 1. + dt/volume*flowSum + dt/volume*EXCHANGE_RATE_VESSELS_INTERSTITIAL;

			// c_P,i^n+1 (for all neighbors i)
			/*for( int v=0; v<this->vesselNodes[i]->countNeighbors; v++)
			{
				float flow;
				// in flow
				if( this->vesselNodes[i]->pressure < this->vesselNodes[i]->neighbors[v]->pressure){
					flow = -fabs(this->vesselNodes[i]->branches[v]->flow);
					sA->set(i,this->vesselNodes[i]->neighbors[v]->index, flow*dt/r/volume);
				}
			}
			*/

			// c_I^n+1
			int index = this->vesselNodes[i]->position[0]
			          + this->vesselNodes[i]->position[1]*DOMAIN_SIZE_X
			          + this->vesselNodes[i]->position[2]*DOMAIN_SIZE_X*DOMAIN_SIZE_Y;
			sA->set(i,M+index, -EXCHANGE_RATE_VESSELS_INTERSTITIAL*dt/r/volume);

			// b
			//b[i] = 1./r * this->vesselNodes[i]->marker;
			b[i] = 1./r * this->vesselNodes[i]->marker + dt/r * ARTERIAL_MARKER_CONC * flowSum/volume;
		}
	}

	// Construct Matrix for Interstitial Marker Concentration
//#pragma omp parallel for
	for( int z=0; z<DOMAIN_SIZE_Z; z++)
		for( int y=0; y<DOMAIN_SIZE_Y; y++)
			for( int x=0; x<DOMAIN_SIZE_X; x++)
			{
				int i = M
					  + x
				      + y*DOMAIN_SIZE_X
				      + z*DOMAIN_SIZE_X*DOMAIN_SIZE_Y;

				// volume: tissue
				VesselNode *vn = this->octree->at( x,y,z);
				float volume = LATTICE_CONSTANT*LATTICE_CONSTANT*LATTICE_CONSTANT;
				if( vn!=0)
				for( int v=0; v<vn->countNeighbors; v++)
					volume -= 0.5*LATTICE_CONSTANT
						* PI * vn->branches[v]->radius*vn->branches[v]->radius;

				if(volume < 0.){
					fprintf( stderr, "ERROR: negative volume: %lf!!\n", volume);
					fprintf( stderr, "ERROR: max volume: %lf = %lf^3\n", (float)LATTICE_CONSTANT*LATTICE_CONSTANT*LATTICE_CONSTANT, (float)LATTICE_CONSTANT);
					for( int v=0; v<vn->countNeighbors; v++)
						fprintf( stderr, "%i neighbor: radius=%lf, volume=%lf\n", v, vn->branches[v]->radius, 0.5*LATTICE_CONSTANT	* PI * vn->branches[v]->radius*vn->branches[v]->radius);
					exit(0);
				}

				// c_I^n+1
				sA->set(i,i, 1);

				// r
				//float r=(1.-volume)/dt + EXCHANGE_RATE_VESSELS_INTERSTITIAL;
				float r = 0.;
				for( int nx=(x>0?x-1:x+1); nx<=x+1 && nx<DOMAIN_SIZE_X; nx+=2)
					r+=1;
				for( int ny=(y>0?y-1:y+1); ny<=y+1 && ny<DOMAIN_SIZE_Y; ny+=2)
					r+=1;
				for( int nz=(z>0?z-1:z+1); nz<=z+1 && nz<DOMAIN_SIZE_Z; nz+=2)
					r+=1;
				r = 1.
				  + dt * MARKER_DIFFUSION_COEFFICIENT/(LATTICE_CONSTANT*LATTICE_CONSTANT) * r
				  + dt * EXCHANGE_RATE_VESSELS_INTERSTITIAL/volume;

				// c_I,i^n+1 (for all neighbors i)
				for( int nx=(x>0?x-1:x+1); nx<=x+1 && nx<DOMAIN_SIZE_X; nx+=2)
				{
					int ii = M
						   + nx
					       + y*DOMAIN_SIZE_X
					       + z*DOMAIN_SIZE_X*DOMAIN_SIZE_Y;
					sA->set(i,ii, -MARKER_DIFFUSION_COEFFICIENT/(LATTICE_CONSTANT*LATTICE_CONSTANT) * dt/r);
				}
				for( int ny=(y>0?y-1:y+1); ny<=y+1 && ny<DOMAIN_SIZE_Y; ny+=2)
				{
					int ii = M
						   + x
					       + ny*DOMAIN_SIZE_X
					       + z*DOMAIN_SIZE_X*DOMAIN_SIZE_Y;
					sA->set(i,ii, -MARKER_DIFFUSION_COEFFICIENT/(LATTICE_CONSTANT*LATTICE_CONSTANT) * dt/r);
				}
				for( int nz=(z>0?z-1:z+1); nz<=z+1 && nz<DOMAIN_SIZE_Z; nz+=2)
				{
					int ii = M
						   + x
					       + y*DOMAIN_SIZE_X
					       + nz*DOMAIN_SIZE_X*DOMAIN_SIZE_Y;
					sA->set(i,ii, -MARKER_DIFFUSION_COEFFICIENT/(LATTICE_CONSTANT*LATTICE_CONSTANT) * dt/r);
				}

				// c_P^n+1
				if( vn!=0)
					sA->set(i,vn->index, -EXCHANGE_RATE_VESSELS_INTERSTITIAL/volume * dt/r);

				// b
				b[i] = 1./r * markerIntSpaceIn[x][y][z];
			}


	//SolveBiCGSTAB( sA, b, x, 20000, 1e-10);
	//ConjugateGradientSparse( sA, b, x, 20000, 1e-20);
	Solver<float> *S = new Solver<float>( M+N, Solver<float>::BiCGSTAB);
	S->Solve( sA, b, x);
	delete S;

	for( int i=0; i<M; i++)
		this->vesselNodes[i]->marker = x[i];
	for( int iz=0; iz<DOMAIN_SIZE_Z; iz++)
		for( int iy=0; iy<DOMAIN_SIZE_Y; iy++)
			for( int ix=0; ix<DOMAIN_SIZE_X; ix++)
			{
				int i = M
					  + ix
				      + iy*DOMAIN_SIZE_X
				      + iz*DOMAIN_SIZE_X*DOMAIN_SIZE_Y;
				markerIntSpaceOut[ix][iy][iz] = x[i];
			}


	delete sA;
	free(b);
	free(x);
}


void VesselGraph::updateSegmentTypes()
{
	int set = 0;

	// Set Roots
	for(int vn=0; vn<this->countVesselNodes; vn++){
		if(this->vesselNodes[vn]->getType() == ROOT){
			if( this->vesselNodes[vn]->pressure == MAX_PRESSURE){
				for( int b=0; b<this->vesselNodes[vn]->countNeighbors; b++)
					this->vesselNodes[vn]->branches[b]->type = ARTERIE;
			}else{
				for( int b=0; b<this->vesselNodes[vn]->countNeighbors; b++)
					this->vesselNodes[vn]->branches[b]->type = VENE;
			}
		}
	}

	// Remaining
	do{
		set=0;

		for(int vs=0; vs<this->countVesselSegments; vs++){
			for( int b=0; this->vesselSegments[vs]->type == UNKNOWN && b<this->vesselSegments[vs]->vesselNodes[0]->countNeighbors; b++)
				if( this->vesselSegments[vs]->vesselNodes[0]->branches[b]->type != UNKNOWN)
					vesselSegments[vs]->type = vesselSegments[vs]->vesselNodes[0]->branches[b]->type;
			for( int b=0; this->vesselSegments[vs]->type == UNKNOWN && b<this->vesselSegments[vs]->vesselNodes[1]->countNeighbors; b++)
				if( this->vesselSegments[vs]->vesselNodes[1]->branches[b]->type != UNKNOWN)
					vesselSegments[vs]->type = vesselSegments[vs]->vesselNodes[1]->branches[b]->type;

			if( this->vesselSegments[vs]->type != UNKNOWN)
				set++;
		}
	}while( set<this->countVesselSegments);
}

void VesselGraph::updateRadius()
{
	int set = 0;
	for( int i=0; i<this->countVesselSegments; i++)
		if( this->vesselSegments[i]->vesselNodes[0]->getType() != TIP && this->vesselSegments[i]->vesselNodes[1]->getType() != TIP)
			this->vesselSegments[i]->radius = 0;
		else{
			this->vesselSegments[i]->radius = MIN_VESSEL_RADIUS;//*LATTICE_CONSTANT/6.;
			set++;
			//fprintf( stderr, "INFO: set segment %i\n",this->vesselSegments[i]->index);
		}
	//fprintf( stderr, "INFO: set=%i\n",set);
	bool change;
	float alpha = 2.;//2.7;//6.;//2.7;
	do{
		fprintf( stderr, "\rupdateRadius: %.2f\%% \b", (float)set/(float)this->countVesselSegments*100.);
		change = false;
		int i=0;
//#pragma omp parallel for
		for( i=0; i<this->countVesselNodes; i++){
			if( this->vesselNodes[i]->getType() != TIP && this->vesselNodes[i]->countNeighbors>1/*&& this->vesselNodes[i]->radius == 0*/){
				float radius = 0;
				int countNonSet = 0;
				int index = -1;
				for( int b=0; b<this->vesselNodes[i]->countNeighbors; b++){
					if( this->vesselNodes[i]->branches[b]->radius != 0){
						radius += pow(this->vesselNodes[i]->branches[b]->radius, alpha);
					}else{
						index = this->vesselNodes[i]->branches[b]->index;
						countNonSet++;
					}
				}
				if( countNonSet==1){
					//fprintf( stderr, "INFO: Set Radius of Segment %i\n", index);
					this->vesselSegments[index]->radius =
							MIN( MAX_VESSEL_RADIUS, pow(radius, 1./alpha));
					change = true;
					set++;
					/*fprintf( stderr, "INFO: set segment %i (node %i) -> radius=%lf\n",
							this->vesselNodes[i]->index,
							this->vesselSegments[index]->index,
							this->vesselSegments[index]->radius);*/
				}
			}
		}
	}while(change /*|| set!=this->countVesselSegments*/);

	if(set!=this->countVesselSegments){
		this->printToEPS( "test.eps", 0 ,0);
		fprintf( stderr, "INFO: set=%i != this->countVesselSegments=%i\n", set, this->countVesselSegments);
		exit( 0);
	}
			
}


VesselNode::VesselNode(int x, int y, int z)
{
	this->position[0] = x;
	this->position[1] = y;
	this->position[2] = z;
	this->neighbors = 0;
	this->branches = 0;
	this->countNeighbors = 0;
	
	this->pressure = 0.;
	this->time = 0.;
	this->marker = 0.;

	this->type = 0;
	this->index = -1;
}

void VesselNode::addNeighbor(VesselNode *neighbor)
{
	this->neighbors = (VesselNode**) realloc( this->neighbors, sizeof(VesselNode*) * (this->countNeighbors+1));	
	this->neighbors[(int)this->countNeighbors] = neighbor;
	countNeighbors++;
}

void VesselNode::removeNeighbor(VesselNode *neighbor)
{
	int n=0;
	for( ; n<this->countNeighbors && this->neighbors[n] != neighbor; n++) ;

	if( n<this->countNeighbors){
		countNeighbors--;
		this->neighbors[n] = this->neighbors[(int)this->countNeighbors];
		this->neighbors = (VesselNode**) realloc( this->neighbors, sizeof(VesselNode*) * (this->countNeighbors));
		this->branches[n] = this->branches[(int)this->countNeighbors];
		this->branches = (VesselSegment**) realloc( this->branches, sizeof(VesselSegment*) * (this->countNeighbors));
	}
}

void VesselNode::addNeighbor(VesselNode *neighbor, VesselSegment *vs)
{
	this->neighbors = (VesselNode**) realloc( this->neighbors, sizeof(VesselNode*) * (this->countNeighbors+1));	
	this->neighbors[(int)this->countNeighbors] = neighbor;
	this->branches = (VesselSegment**) realloc( this->branches, sizeof(VesselSegment*) * (this->countNeighbors+1));	
	this->branches[(int)this->countNeighbors] = vs;
	countNeighbors++;
}

/*void VesselNode::addSegment(VesselSegment *vs)
{
	this->branches = (VesselSegment**) realloc( this->branches, sizeof(VesselSegment*) * (this->countNeighbors+1));	
	this->branches[(int)this->countNeighbors] = vs;
	countNeighbors++;
}*/

VesselSegment::VesselSegment(VesselNode *node1, VesselNode *node2)
{
	this->vesselNodes[0] = node1;
	this->vesselNodes[1] = node2;
	//this->vesselNodes[0]->addNeighbor(this->vesselNodes[1], this);
	//this->vesselNodes[1]->addNeighbor(this->vesselNodes[0], this);
	
	this->flow = 0;
	this->shear = 0;
	this->radius = 4;
	this->wallThickness = 1;

	this->type = UNKNOWN;
}

VesselSegment::~VesselSegment()
{
	//this->vesselNodes[0]->removeNeighbor(this->vesselNodes[1]);
	//this->vesselNodes[1]->removeNeighbor(this->vesselNodes[0]);

	//this->radius = 4;
	//this->wallThickness = 1;
}

VesselNode::~VesselNode()
{
	free( neighbors);
	free( branches);
}

char VesselNode::getType()
{
	return type;
}

void VesselNode::setType( char type)
{
	if( this->type == ROOT){
		//fprintf( stderr, "Set Node %i (%i,%i) from %i -> %i\n", this->index, this->position[0], this->position[1], this->type, type);
		fprintf( stderr, "Set Node %i", this->index);
		fprintf( stderr, " (%i,%i) from", (int)this->position[0], (int)this->position[1]);
		fprintf( stderr, " %i", this->type);
		fprintf( stderr, " -> %i\n", type);
		exit( 0);
	}
	
	//if( type == ROOT)
	//	fprintf( stderr, "Set Node %i (%i,%i) from %i -> %i\n", this->index, (int)this->position[0], (int)this->position[1], this->type, type);
		
	this->type = type;
}

double parkerAIF(
		double t,
		double A,
		double T,
		double sigma,
		double alpha,
		double beta,
		double tau,
		double s)
{
	return A/(sigma*sqrt(2.*PI)) * exp(-(pow(t-T,2)) / 2./pow(sigma,2.)) + alpha * exp(-beta*t) / (1.+exp(-s*(t-tau)));
}
double VesselGraph::AIF::ArterialInputFunction( //mM
		double time,  //min
		double time0, //min
		const char type)
{
	//return 1.;

	double A1 = 0.809;
	double A2 = 0.330;
	double T1 = 0.17046;
	double T2 = 0.365;
	double sigma1 = 0.0563;
	double sigma2 = 0.132;
	double alpha = 1.050;
	double beta = 0.1685;
	double s = 38.078;
	double tau = 0.483;
	double t = time - time0;
	if( t < 0)
		return 0.;
	else
		return parkerAIF(t,A1,T1,sigma1, alpha,beta,tau,s) + parkerAIF(t,A2,T2,sigma2, alpha,beta,tau,s);
}
